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Abstract 

This paper describes a new statistical, model-based approach to building a contact 
state observer. The observer uses measurements of the contact force and position, 
and prior information about the task encoded in a graph, to determine the current 
location of the robot in the task configuration space. Each node represents what 
the measurements will look like in a small region of configuration space by stor- 
ing a predictive, statistical, measurement model. This approach assumes that the 
measurements are statistically block independent conditioned on knowledge of the 
model, which is a fairly good model of the actual process. Arcs in the graph rep- 
resent possible transitions between models. Beam Viterbi search is used to match 
the measurement history against possible paths through the model graph in order to 
estimate the most likely path for the robot. The resulting approach provides a new 
decision process that can be used as an observer for event driven manipulation pro- 
gramming. The decision procedure is significantly more robust than simple threshold 
decisions because the measurement history is used to make decisions. The approach 
can be used to enhance the capabilities of autonomous assembly machines and in in 
quality control applications. 
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Notation 



The notation in this thesis tries to follow the conventions of the International Journal 
of Robotics Research. Scalar constants and variables are typeset in plain math italics. 
Vectors and matrices are typeset in bold math italics. Sets are typeset in calligraphic. 
Functions are typeset according to the type that they return. 

This thesis uses concepts from statistics, differential geometry, and robotics so there 
is quite a bit of notation. This sheet provides a reference to the meaning of each 
symbol. 

Below is the configuration space and planning notation. This notation is primarily 
introduced in chapter 2. 

C Configuration space 

T Free space 

O The obstance space 

<f> Empty set or space 

X The robot's task state space 

x The current robot state or configuration 

v Generalized velocity of the robot 

Q A goal subset of C 

M. The measurement function 

T Forward projection operator 

B Backward projection operator 

U Control space 

u Generalized control 

K, Interpretation set 



Below is the differential geometry notation. This notation is primarily introduced in 
chapter 4. 

M. A manifold 

T(Ai) The tangent bundle of a manifold M. 

T x (Ai) The tangent space at x of M. 

T*(Ai) The cotangent bundle of a manifold M. 

T-^(Ai) The cotangent space at x of a manifold M. 

V Phase space = C X TiC) 

g Geometric parameters of a constraint equation 

C(g,x) A contact constraint equation 

C x Partial derivative of C with respect to x 

C* Complement of C x and a basis for the cotangent bundle 

Below is notation about the robot joint and endpoint coordinates and dynamics. 
This notation is introduced primarily in chapter 4. 

dx Differential endpoint or state-space motion 

x Endpoint velocity of the robot 

x Endpoint acceleration of the robot 

q Robot joint coordinates 

dq Differential joint motion 

q Robot joint velocity 

q Robot joint acceleration 

H Generalized mass or inertia 

Cor Coriolis acceleration 

K Generalized spring 

B Generalized damper 

p Generalized momentum 



Below is the measurement and statistical notation. A standard is that any symbol 
with a hat over it is an estimate of the value of the symbol, and any symbol with a 
tilde over it is the difference between the estimate and the true value of the symbol. 

y Measurement vector 

y Predicted measurement 

w m Generalized force measurement 

s The strain measurements 

v The vibration measurements 

n Estimated normal to a surface 

h Observer function 

P(x) Probability of an event 

p(x) Probability density function 

E Expected value 

V Variance or covariance of a variance 
H 8 - Hypothesis i 

N Standard normal distribution 

U Uniform distribution over a set 

Exp Exponential distribution 

ME Maximum entropy distribution over a set 

X Chi-square distribution 

X Measured empirical covariance 

i.i.d. Independent identically distributed 

L Likelihood of an event 

/ Log-likelihood of an event 

6 Parameter vector of a distribution 

v White residual process from an estimator 

dv White Wiener process 

Below is the feature notation. 

M Feature nodes 

£ Feature edges 

Q Feature graph = (A/", Q) 

V Feature partition 



Introduction 



Chapter 1 



A basic problem of autonomous manipulation and semi-autonomous telerobotics is 
to perform a basic control primitive: move object until some condition is satisfied. 
In fine manipulation, the primitives may be defined in terms of contact constraints 
and forces on the object. For example, we might like to instruct a Mars rover to 
probe for hard rocks using a stick-like probe. In assembly, we would like to instruct 
a robot to mate object A with object B. As a last example, a maintenance robot 
might be instructed to tighten a fastener until "it is tight". The facility with which 
humans perform all these tasks hides the complexity and detail involved in these 
simple instructions. 

Each of these tasks involves the interaction of a grasped part with the environment. 
Therefore, the contact forces between the part and the environment cannot be di- 
rectly measured. Instead, the interaction state must be inferred from sensors in the 
robot wrist or end-effector. In addition, often in robotic grasping the end-effector 
cannot directly measure grasp forces. Instead force and torque sensors are placed 
in the wrist or fingertips which are used to infer the grasp force and thereby the 
current grasp state. Therefore, an understanding of perception which helps to track 
the progress of mating tasks using force sensing has wide applicability in robotic 
manipulation. 

These manipulation tasks are usually accomplished by a sequence of steps. Generally 
each step reduces the relative positioning error between the manipulated parts by 
incrementally adding contact constraints. Each step is usually a compliant motion 
and the sequence of motions is indexed by some termination predicate. The sequence 
of steps may depend upon the effects of the interaction. Since there can be multiple 
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outcomes in any given interaction, branching and context based decision making is 
always necessary. 

For example, one strategy for peg-in-hole assembly is to bring the the peg into contact 
with the top surface. Then a new motion is used to place the tip of the peg in the 
hole. Then the peg is approximately aligned with the hole. Finally, an insertion step 
is used to push the peg into the hole. 

Multi-step strategies also occur in grasping. As an example consider the experiment 
of grasping, lifting, and replacing an object of unknown mass given by [Howe et 
al., 1990]. In their paper, the sensors measured fingertip forces, accelerations, and 
relative slip while a two finger grasper was used to grasp, lift, and replace a single 
test object. Six distinct control steps, or phases, were determined for this task: 
pre-contact, loading, manipulation, unloading, post-contact, and slip. Each phase 
required a different control algorithm, and termination of each phase was signaled 
by different signal predicates. 

The idea is to connect a set of continuous control algorithms into a discrete graph. 
Transitions in the graph are controlled by a decision process that uses the measure- 
ments and possibly the controls. A collection of thresholds can serve as a simple, but 
not very robust, decision procedure. When the measurement cross these thresholds 
a contact event is declared to have occured. The resulting controller is a mixture 
of continuous controllers embedded within a nondeterministic finite state machine. 
The algorithm which observers the measurements to detect contact events, or to 
determine the contact state is a contact state observer. 

This idea of event driven manipulation programming has been evolving under dif- 
ferent names at many research centers. Brooks [Brooks, 1985, Brooks, 1987] argues 
for an implementation called the subsumption architecture. Howe and Cutkosky 
define the events by the measurements generated as the robot transitions between 
manipulation phases. They used these events to drive their grasping algorithm. An- 
other work [McCarragher and Asada, 1993b] focused on the transitions to drive an 
assembly algorithm. 

In most of the previous work, the contact state observer has taken the form of a set of 
thresholds. For unstructured manipulation this level of perception is not sufficient. 
Uncertainty, noise, and the range and frequency of contact events makes observing 
the contact state difficult. More powerful statistical tools are needed to formulate 
and solve the observer problem. 
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Hannaford's [Hannaford and Lee, 1991] use of Hidden Markov Models (HMM) is 
one of the few formulations of the observer as a statistical decision problem. They 
modeled the sequence of forces that occur during a given manipulation using a HMM 
for the force. Although highly effective for repetitive problems, such as assembly, the 
approach may not be able to deal with less structured problems because in general 
the forces depend upon the commands sent to the robot. 

More recently [Delson, 1994] looked at programming a robot using human demon- 
stration. As part of this work he developed some tests for segmenting the motion 
data into section that come from a single contact configuration. The tests were done 
either only on the force or on the velocity depending upon the contact condition. 
This thesis significantly extends his work by explicitly modeling the process noise, 
making decisions on all of the data, making all the decisions on both the position 
and the force information, and creating a framework that can incorporate more than 
just constraint models. 

This thesis presents a new statistical, model-based approach to building contact state 
observers. The hrst part of the thesis shows how the measurements of forces and po- 
sitions produced in a given task can be described as a graph of predictive, statistical, 
measurement models. Each model describes statistics of the measurements that are 
intrinsic to the task and which are not functions of the applied command. 

The second part of the thesis shows how the graph and the measurement models can 
be used as basis for constructing a contact state observer. Because of the statistical 
formulation of the measurement models, we can formulate the observer as a statis- 
tical decision problem. The result is that the observer is simply a search procedure 
which attempts to determine a collection of statistically most likely paths for the 
measurements through the graph and thereby observers the contact state. 

This approach can produce timely results and incorporate global context. The ap- 
proach also makes explicit use of the complete measurement history. Decisions are 
made dynamically by comparing one possible model path for the measurements to 
other model paths. This eliminates the problems inherent in making fixed threshold 
decisions on a fixed, generally short, length of data. In addition, since decision tests 
are derived from models, the assumptions made in picking decision thresholds are 
explicit. 

Figure 1.1 shows the essential idea of this thesis. The figure shows how the infor- 
mation available to a robot with an endpoint force sensor in a typical task can be 
represented by a collection of constraint models. 
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The upper-left figure shows a circular robot constrained by a maze with five legs. 
The position and orientation of the maze are only approximately known. This figure 
pictorially represents a spherical fingertip that was attached to a three degree-of- 
freedom robot, the PHANToM, that was used to experimentally verify the theory. 
The PHANToM is shown in the maze in figure 1.2. A six axis force-torque sensor, 
shaped like a fingertip, is attached at the end of the PHANToM. Encoders on the 
PHANToM's motors are used to measure the Cartesian position of the fingertip. 
The measured position was hrst order differenced and lowpass filtered to compute 
the fingertip velocity. 

The constraints between the robot and the maze are best represented by the robot's 
configuration space. The configuration space, or c-space, represents all the locations 
the robot's reference frame can be placed without causing a collision. The c-space is 
shown in the lower-left of figure 1.1. The c-space representation shows that the maze 
is representative of may assemblies. Many real assemblies look like tubes or mazes 
when the c-space is represented in the correct coordinates. 

Constant velocity commands can be applied to the robot to move it through the 
maze. The lower-left of figure 1.1 shows a motion from the bottom-left leg to the 
upper-middle leg resulting from the indicated velocity command. Part of the path 
is highlighted in the figure. 

In all the experiments, the force sensor measured the three Cartesian contact forces 
between the robot and the maze, and the position sensor measured the Cartesian 
position of the robot relative to the robot's fixed ground. The upper-right of figure 
1.1 shows a sequence of forces and velocities measured by the sensors for one trial 
for this example path. 

The contact state observer problem is to determine from knowledge of the c-space 
and force and position measurements where the robot is in the maze. The force trace 
shows why this is a difficult problem. There is considerable noise due to vibration 
and friction stick-slip displayed in the force signal. Similar noise also appears on the 
velocity signal. Both signals are statistically non-stationary, and there are several 
short time impact events in the force signal. 

The bottom-right figure of figure 1.1 shows the central representation idea of this 
thesis. Each contact in the c-space is represented as a constraint model. The robot 
starts out in the lower-left corner of the c-space and this is represented by the node 
showing the corner contact. The motion command causes the robot to move into 
free space and then against the opposite wall. This is shown by the empty node 
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Figure 1.1: Example of information available in contact sensing for a 
circular robot in a maze, and the representation of the task in terms of 
a graph of constraint models. 
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Fingertip Sensor 



Figure 1.2: The PHANToM haptic interface with a force sensing fin- 
gertip. This was the experimental apparatus used in the experiments 
in this thesis. 



followed by the point in contact with the single wall. In this way each of the nodes 
represents a single, continuous, constraint situation in the c-space. The connection 
of the nodes comes from the connections caused by the indicated command in the 
underlying c-space. 

Each of these nodes represents a single contact situation for which a predictive sta- 
tistical measurement model can be created. The predictions can be used by a search 
procedure to determine the best marking of the measurement sequence against pos- 
sible paths through the graph of models. The best marking for this example path 
is shown in the upper-right figure. A procedure which finds the best marking is in 
effect a contact state observer. 

Other models, such as models for the vibration level, spectral shape of the vibration, 
or impacts can also be added into the graph. Transitions from free space into a 
contact situation can give rise to impact events. These appear in the force trace, and 
detecting these events can help in tracking motion through c-space. 



Figure 1.3 shows the basic components of the contact state observer. The forces and 
positions are input into a collection of candidate estimators. There is one estimator 
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Figure 1.3: Contact state observer processing. The figure shows the 
basic architecture of the contact state observer processing force and 
position information measured by the robot as it explores the maze. 



for each hypothesized contact model. The estimators output a measure of match 
between the underlying model and measurements called the log-likelihood. 1 The 
estimators also output a residual process, which conditioned on the model being true, 
is a white noise sequence. The residual processes gets fed into a change detector. The 
change detector monitors the residuals for changes. The change detector computes 
the log-likelihood of departure from the current model and the most likely time 
of departure. Both of these statistics, and the model likelihoods computed by the 
estimators, are sent into a single decision procedure. This procedure uses all these 
process statistics and the graph to determine a collection of likely paths for the 
measurements through the graph using a search strategy. Since each of these paths 
terminates in a state, the procedure also computes an estimate of the current state 
of the robot. Therefore, this process reduces the problem of observing the contact 
state to the problem of observing the current contact model. 

The coding of the task, and all of the modeling work lies in determing appropriate 
measurement models and how they should be related given the geometry and applied 
command. These measurement models are called contact features or just features. 
The rest of the procedure follows from the specified process statistics. 

The rest of the thesis presents the components of the observer in detail and a demon- 



^■The likelihood of a measurement is the conditional probability or density for the measurement 
given the parameters of the statistical model. The log-likelihood is the logarithm of the likelihood. 
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stration using the maze is developed to illustrate all the components acting together. 
The hrst component is the collection of measurement models and their associated es- 
timators. The hrst model is a time series model for a vibration measurement formed 
from the force measurements. The second measurement model is a model of con- 
straint. We present the estimators, indicate performance, and indicate how they can 
be used for selecting the most likely model for a given batch of measurements. 

Of course real data comes from a sequence of measurement models. It is therefore 
essential to be able to detect when a model has changed. This is the problem of 
segmentation or change detection. Lastly, the contact state observer is presented. 
The observer simultaneously segments and labels measurement sequences in order to 
determine an estimate of the robot's state. 

The resulting complete observer can be applied to robot programming in order to: 
1) mediate multiple step robot motions, 2) detect completion of a motion, 3) recover 
from unexpected contact events, and 4) detect model errors. The new techniques 
presented in this thesis are based on two insights: 1) additional, useful, manipulation 
sensing primitives exist in the force and position signals caused by contact other 
than just the raw force, and 2) sequential estimation and decision theory provides a 
powerful tool for detecting and isolating these primitives. 

The core thesis contributions are: 

1. Development of a useful model-based definition of contact features. 

2. Development of a multi-outcome representation, the task feature graph, for 
relating features to steps in a task. 

3. Development of efficient, robust, model-based, maximum likelihood (MLE) fea- 
ture estimation, and segmentation techniques, for temporal and contact con- 
straint features. 



1.1 Thesis Approach 



There are two unifying threads in the thesis which are used many times. The hrst 
thread comes from the ideas of statistical modeling and decision making. We model 
the basic problem of contact sensing in terms of determing the current measurement 
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model for the contact force and position measurements. Because we start from a sta- 
tistical representation, we are able to utilize the powerful tools that have been devel- 
oped for stochastic measurement processes. Maximum likelihood estimation [Ljung, 
1983] is used for extracting the feature parameters. Sequential hypothesis tests, 
originally developed by Wald [Wald, 1947], are used for on-line segmentation of the 
measurement processes into a sequence of measurement models. 

The second important thread comes from dynamics in configuration space [Lozano- 
Perez, 1983, Arnold, 1989] . Configuration space is the space of all possible generalized 
coordinates, or configurations, for the moving robot. Manipulation and mating are 
processes which are best modeled as motions in configuration space. The contact 
forces in manipulation are configuration space constraint reaction forces. 

Although the contact feature sensing framework developed in this thesis can handle 
other information, this thesis focuses on the information available from measurements 
of force and position for a single end-effector. Position and force information is all the 
contact measurement information available in mating tasks. A good representation 
and perception of this information is essential to manipulation since mating is major 
component of active manipulation. These measurements provide information about 
the geometry of configuration space and the location of the robot in configuration 
space. 

Therefore, three ideas come from the study of configuration space : 



1. The process dynamics, and the applied command, provide a notion of connec- 
tivity in phase-space which is used to build the feature graphs. 



2. Forces applied against contact constraints, represented in configuration space, 
produce the low frequency, quasi-static component of the measured contact 
forces. The form of these configuration space constraints must be understood 
in order to interpret the measured forces. 



3. Motions along textured constraint surfaces, or changes in the forms of the con- 
straint, produce distinct dynamic contact events. The source of these events is 
best represented by motions along configuration space surfaces, and discontin- 
uous changes in the surface geometry. 
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1.2 Guide to Thesis 



The thesis has been broken down into four broad sections. Chapters 2 and 3 provide 
background on the problem of manipulation programming and contact sensing. This 
area has been of major interest to robotics researchers for many years. Therefore, 
chapter 2 provides a historical review of the problem of sensor guided manipulation 
programming. The historical review provides a motivation for the idea of feature 
based programming. Feature based programming uses local controllers, which are 
selected by determining the active contact feature. The approach rests on ideas of 
feedback control and observer theory. 

Chapter 3 discusses manipulation cues in general, before focusing on the cues avail- 
able from internal force and position measurements. This subset of contact sensing 
has been called intrinsic contact sensing; and the relation of this thesis to that work 
is discussed here. 

Chapters 4 and 5 form the bulk of the contact feature sensing theory. Chapter 4 re- 
views configuration space dynamics. Some basic concepts from differential geometry 
are presented in order to facilitate work in later chapters. This chapter essentially 
presents configuration space as a framework for understanding intrinsic contact in- 
formation. It can be skimmed and referred to as needed. 

Chapter 5 defines contact features in terms of the measurement models and then 
uses the definition to create several graph structures useful for sensing task progress. 
In particular, the feature graph is defined using forward projection. Chapter 6 then 
presents a contact feature observer. Maximum likelihood estimation and sequential 
hypothesis testing are used to develop the observer. 

Chapters 7, 8, and 9 apply the general theory to develop particular results useful 
for rigid body manipulation. Chapter 7 considers the problem of temporal feature 
estimation and segmentation. This chapter shows how impacts, and changes in signal 
spectrum, can be detected. Chapter 8 develops a novel constraint estimation scheme 
which is able to incorporate all the available contact information for finite rotations 
in the plane. Extensions of the ideas to semi-rigid bodies are also discussed. Finally, 
chapter 9 uses the results of chapters 7 and 8, in order to demonstrate the general 
theory. 

Chapter 10 concludes by summarizing the results of the thesis, and presenting ideas 
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for future work. 
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Manipulation Programming 



Chapter 2 



A basic problem of autonomous manipulation and semi-autonomous telerobotics is 
to perform a basic control primitive: move object until some condition is satisfied. 
In fine manipulation, the primitives may be defined in terms of contact constraints 
and forces on the object. For example, we might like to instruct a Mars rover to 
probe for hard rocks using a stick-like probe. In assembly, we would like to instruct 
a robot to mate object A with object B. As a last example, a maintenance robot 
might be instructed to tighten a fastener until "it is tight". The facility with which 
humans perform all these tasks hides the complexity and detail involved in these 
simple instructions. 

Each of these tasks involves the interaction of a grasped part with the environment. 
Therefore, the contact interaction forces cannot be directly measured. Instead, the 
form and state of this interaction must be inferred from sensors in the robot wrist or 
end-effector. In addition, often in robotic grasping the end-effector cannot directly 
measure grasp forces. Instead force and torque sensors are placed in the wrist or 
fingertips which must be used to infer the grasp force and thereby the current grasp 
state. Therefore, an understanding of perception which helps to track the progress 
of mating tasks has wide applicability in robotic manipulation. 

Manipulation tasks like these are termed fine manipulation problems because the mo- 
tions involved are usually small. A fine manipulation task is usually accomplished 
by a sequence of steps. Generally each step reduces the relative positioning error 
between the manipulated parts by incrementally adding contact constraints. Con- 
straints are added incrementally because the approach has a much higher probability 
of succeeding than a strategy which attempts to go from no constraint to full con- 
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straint in one step. For example, one strategy for peg-in-hole assembly is to bring 
the the peg into contact with the top surface. Then a new motion is used to place 
the tip of the peg in the hole. Then the peg is approximately aligned with the hole. 
Finally, an insertion step is used to push the peg into the hole. 

Each step is usually a compliant motion 1 and the sequence of motions is indexed 
by some termination predicate. The sequence of steps may depend upon the effects 
of the interaction. Since there can be multiple outcomes in any given interaction 
branching and context based decision making is always necessary. 

Multi-step strategies also occur in grasping. As an example consider the experiment 
of grasping, lifting, and replacing an object of unknown mass given by [Howe et al., 
1990]. An idealized version of this task is shown in figure 2.1. In their paper, the 
sensors measured fingertip forces, accelerations, and relative slip while a two finger 
grasper was used to grasp, lift, and replace a single test object. Six distinct control 
steps, or phases, were determined for this task: pre-contact, loading, manipulation, 
unloading, post-contact, and slip. Each phase required a different control algorithm, 
and termination of each phase was signaled by different signal predicates. 

The idea is to connect a set of continuous control algorithms into a discrete network. 
Transitions in the network are controlled by a decision process that uses the mea- 
surements and possibly the controls. If the decision process only sets thresholds on 
the current values of the measurements, the resulting controller is a mixture of con- 
tinuous controllers embedded within a nondeterministic finite state machine. More 
general structures are also possible. 

This idea of event driven manipulation has been evolving under different names at 
many research centers. Nevins et al [Nevins et al., 1973] presents one of the earliest 
event driven frameworks for programming a robot for assembly. Brooks [Brooks, 
1985, Brooks, 1987] argues for an implementation called the subsumption architec- 
ture. Howe and Cutkosky use event transitions, which they term contact events, 
to drive their grasping algorithm. Another work [McCarragher and Asada, 1993a] 
focused on the transitions to drive an assembly algorithm. 

This mixed approach of continuous control within discrete domains or contexts is 
very appealing. It breaks the problem down into components which can then be 
solved in isolation. By appropriately combining solutions, more complex behaviors 



Compliance control or force control has a long history in robotics. [Whitney, 1985] provides 
a historical perspective of force control techniques which have since become standard and can be 
found in textbooks such as [Craig, 1989]. 
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Figure 2.1: An idealized version of the lifting problem treated by Howe 
and Cutkosky. The rollers on the block are frictionless. The weight 
and contact friction coefficient of the block are in general unknown. 



can be synthesized. However, the vast array of possible interactions between the 
robot and the part and problems that can arise in the interaction is currently the 
basic obstacle to this approach. 

Although many single tasks have been programmed by hand for various robots, the 
innumerable number of potential problems makes direct programming of a large 
number of solutions impractical. Decomposing each task into components and se- 
lecting robot-dependent thresholds is a difficult problem for even one task. Instead, 
a method of programming which gives the robot competence in a large class of fine 
manipulation tasks must be found. Possible solutions might use more general repre- 
sentations, involve learning, or be based on some model-based planning. Uncertainty 
is what makes this problem difficult. There are several sources of uncertainty in ma- 
nipulation: 



1. There can be error in both the geometric and the topologic models of objects, 
especially if the model was extracted from vision or touch. 

2. The location of objects in the environment and in the robot's gripper is not 
exactly known. 

3. The parameters describing the physics of interaction, including the coefficient 
of friction, may be only partially known and may not even be directly modeled. 

4. The control is subject to random disturbances. 
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5. The effect of the control on the motion of objects may be uncertain or unknown. 



6. Measurements of the position and interaction forces for the moving objects 
suffer from measurement noise and unmodeled effects. 



This thesis looked at two problems that arise within manipulation programming. 
First, how should the notion of a contact event be defined so that the discrete control 
networks can be related to task geometry and physics. Second, given an event 
definition how can the discrete state of the robot in the network be estimated or 
observered. An observation of the discrete state is necessary in order to select the 
appropriate control algorithm. 

We will define events as statistically significant deviations from model predictions. 
Our approach is model-based. For every contact situation a statistical model of 
the measurement process is defined. We term these models contact features or just 
features. A model-based approach has several advantages. First, uncertainty is 
explicitly incorporated. The event detectors and observation process can be derived 
directly from the models. Second, models allow some parameterized generalization. 
Third, the representation is amenable to analysis and Monte Carlo simulation in 
order to determine performance. 

Our programming approach is to create a network of measurement models. An ob- 
server is developed to track the state of the robot in this network. The network can 
be hand-programmed, possibly learned, or computed from models and hrst princi- 
ples. Finally, although this is not developed here, it is possible that contexts can be 
defined as collections of nodes in this network and that actions and controllers can 
be created relative to these contexts. The complete approach provides a framework 
for programming robots to solve unstructured tasks. 

Before developing our approach, some preliminaries and background are provided. 
This chapter provides a historical review of treatments of the manipulation program- 
ming problem. This provides some context for our approach. Chapter 3 discusses 
possible manipulation cues and past work on feature sensing. Finally, chapter 4 
gives mathematical background on dynamics and configuration space. This material 
is used to justify the network representation, the feature models, and to develop tools 
for constraint estimation. 
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2.1 History of Fine Manipulation Programming 



The challenge of automatic robot programming for fine manipulation has produced 
a rich history of research in robot design, force control, sensing, and robot planning. 
This section presents some of the more recent approaches to the problem of fine 
motion programming with uncertainty and shows how this thesis relates to that 
work. 



2.1.1 Guarded Moves 



Guarded moves [Bolles and Paul, 1973] is the earliest approach to fine motion pro- 
gramming. A guarded move is a motion which terminates when a force or position 
limit of a certain value is crossed. A branching control strategy can be written by 
combining primitive motions with guarded move termination tests. The combination 
of branching, passive compliance in the form of free uncontrolled joints, and early 
vision incorporated a range of sensing modalities and implicitly anticipated much 
future work in robotics. A similar approach along with the definition of guarded 
moves can be found in [Will and Grossman, 1975]. 

Bolles and Paul presented an assembly demonstration for a model T Ford water 
pump. The water pump base and top were to be assembled with 6 screws and a 
joining gasket. The assembly cell was strongly structured in order to control the 
consequences of uncertainty. Fixtures and alignment blocks were designed to control 
placement errors. Parts were painted white and the background was painted black 
for high contrast vision. Even with all this branching control strategies were still 
needed to deal with the remaining errors. 

One step in the assembly, the insertion of an alignment pin, shows how guarded moves 
can be used to handle and test for errors. Two alignment pins, with a cone insertion 
end, were to be inserted into two of the screw holes in the pump base. Because 
of hxturing, positional errors in the plane are the only errors that can occur. Axis 
angular misalignment is prevented by the fixtures. 

A spiral search strategy is used to compensate for the possible positional error. An 
analysis of the geometry shows that any one attempted insertion can result in three 
possibilities: 1) the pin can go in the hole, 2) the pin can miss the hole and land on 
the top of the base beside the hole, 3) the pin can miss the base. A two part guarded 
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move strategy can be used to discriminate between these cases: 



1. Move down sufficiently to guarantee an insertion will occur. If a large upward 
force is encountered, case 2 must have occurred so continue searching with a 
spiral of ever increasing radius. If the large force is not encountered go to step 
2. 



2. Try and seat the pin in the hole. If resistance (due to friction) is not felt the 
pin must have missed the base so continue the spiral search. If resistance is 
encountered the pin is seated. 



The uses and limitations of guarded moves can be seen in this example. First, 
designing such simple tests for a more complex situation requires analyzing every 
possible contact possibility (and possibly sequence of possibilities) and determining 
a scalar test which can discriminate the cases. In this example there are only two 
degrees-of- freedom that can have errors (because of hxturing). The number of possi- 
ble conditions increases rapidly with the dimension of the underlying space. As part 
of his thesis [Buckley, 1987] determined the number of force or position distinguish- 
able contacts for a three dimensional square peg in a square hole. This study showed 
that there are 1714 possible contact pairings that could occur. Case by case analysis 
of the possible situations is impossible without computer aid. Futhermore, it is very 
unlikely that a single test using only a few force measurements will be sufficient to 
discriminate the cases. 

Second, each test is a very simple threshold on the force. Force is a very noisy signal 
and situations can easily occur which will fool the test. For example, the pin could 
hit the base on the edge of the cone in the second step. The pin would have gone 
in part way in the hrst step, then collision with the cone would cause acceptance on 
the second step. A more complex test that probes the degree and form of constraint 
on the pin would not be confused by such an example. 

Lastly, errors in axis alignment could cause problems with the decisions based on force 
thresholds and would cause problems with control. Axial misalignment (prevented in 
this problem through hxturing) can cause large forces. The remote center compliance 
(RCC) and compliance, or impedance, specification and control was developed to deal 
with small misalignments implicitly using appropriate control. 
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2.1.2 Compliance Specification 



Bolles and Paul anticipate compliance control in their idea of a free joint which is 
aligned with the direction of possible errors. This idea was generalized in the work on 
generalized damper control by [Nevins et al., 1973, Whitney, 1977, Whitney, 1982], 
stiffness control [Salisbury, 1980], impedance control [Hogan, 1985] and hybrid control 
[Craig and Raibert, 1986]. The relationship of compliance specifications to geometric 
constraint was formalized in [Mason, 1981]. A vast amount of additional literature 
has been presented on compliance and force control. 

Compliances are specified relative to a constraint frame. Forces and torques are 
measured and motions are produced relative to this constraint frame. As a strategy, 
compliance control has the advantage that certain types of errors are accommodated 
automatically. For example, angular or translation misalignments can be accommo- 
dated in pin insertion while still controlling the displacement into the hole. Therefore 
it is not necessary to sense errors and switch the control law for a range of errors. 

The complete manipulation will still require branching and a sensor based control 
strategy. The pin insertion task, described above, would still need to use a sequence 
of guarded moves to begin to put the peg into the hole. However once the robot is 
certain the peg is in the hole and beyond the jamming depth, a simple compliant 
insertion will suffice to complete the task. 



2.1.3 Strategy Skeletons 



The first idea for removing some of the burden from the programmer was to write a 
set of strategy skeletons [Lozano-Perez, 1976, Taylor, 1976] which would be available 
as robot programming primitives. There would be a skeleton for maintaining grasp, 
a skeleton for inserting two parts, a skeleton for opening a door, et cetera. Each 
skeleton would have a set of free parameters that would be set when a specific 
problem was to be performed. Skeletons could be written by hand, and then a 
computer program would be used to determine the appropriate parameter values for 
a particular example of a task. 

Taylor wrote such a program. The program used numerical evaluation of part lo- 
cations and errors to propagate the effects of errors and uncertainties through the 
task model. The results were used to fill in the parameters in the skeleton. [Brooks, 



32 Chapter 2: Manipulation Programming 

1982] extended the approach to use symbolic propagation. By back-propagating the 
constraints Irom the goal, constraints on the skeleton parameters could be deduced. 

The skeleton approach rested on the assumption that any one skeleton would provide 
an approach for many examples of a task such as peg-in-hole, close door ect. Each 
skeleton was intended to provide for a range of examples of a task. A few skeletons 
would then suffice to make the robot capable in a given task. The problem is that 
small changes in geometry can necessitate large changes in strategy [Lozano-Perez et 
al., 1984]. Therefore, a single task such as peg-in-hole insertion may require a many 
skeletons. Instead, a technique for directly incorporating geometry and a model of 
the errors is needed for task programming. 



2.1.4 Fine Motion Planning 



Preimage fine motion planning deals with error and the effects of geometry by plan- 
ning over models of the error and geometry [Lozano-Perez et al., 1984]. In this formal 
approach, robot programming is treated as an automatic program synthesis problem 
given knowledge of geometry, physics, and sensing. The programmer writes a single 
program called a planner. Then the planner examines the problem specification in 
terms of the geometry, physics, and possible sensory operations and then computes a 
robot control program which is guaranteed to work. This program is then run on the 
robot. The robot program takes in sensory measurements and outputs appropriate 
controls in order to accomplish the task. Once this single program is written, the 
robot programming problem is solved for all tasks which the planner understands. 
This approach is important because it provides a formal problem statement and 
approach for fine manipulation programming. 

This approach directly incorporates uncertainty. It showed how both contact con- 
straints and sensing could be used to reduce uncertainty. This formulation of the 
control problem is very close to stochastic dynamic programming [Bertsekas, 1976] 
and shares the problem of dimensionality. One approach to solving the problem is to 
discretize the task state space and then consider sequences of motions and sensing 
operations in the discretized state space. Planning must be done over the possible 
sequences of sensing and controls or the knowledge space. 

It will be easier to understand the approach, the complexity of the approach, and 
contrast the approach with other techniques, if we hrst loosely define some terms. 
This presentation will use a discrete representation of time. The state of the robot 
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and all the objects in the environment is specified by a vector of variables x. In 
quasi-static manipulation the state is just the position of the objects, in dynamics 
the velocities are also required. The set of all states is denoted by X . 

The path planning problem, without uncertainty, is the problem of constructing a 
trajectory, or path, through the state space which does not violate any constraints. 
One approach to solving this problem is backchaining from the goal, over single step 
motions. 

The goal Q is a subset of X . The dynamics of the control law, without uncertainty, 
induces a forward projection operation x J+ i = F(xj, Uj) which takes the current state 
Xj and maps it to the next state x J+ i given the control applied at time j, u r The 
control Uj ranges over a set U(x.j). For example, the control might be an increment 
in x that satisfies the constraints. 

The backprojection, without uncertainty, can now be defined. Let Q 3 be the current 
goal set. The backprojection of Q 3 is the set of all points x in X such that there is a 
control in ti{x) which when applied to x will result in an Xj in the goal set. Formally 

B(gj) = {xG X : 3u GW(x)andf(i,u) G Q 3 }. 

Backchaining is now defined by the recursion Q 3 ~\ = B{Gj) with Q the initial goal 
set. 

How complex is this? Let card denote the cardinality of a set. At every stage of the 
backchaining operation, the computer searches over the entire state space (in general) 
and the entire control space in order to determine if there is a control that will reach 
the current goal set. This step is essentially computing controls that connect any 
two states. If this connectivity is memorized, a k step path can be computed in order 
0(k csad(U) csad(X) 2 ) [Erdmann, 1989]. In particular, the problem has polynomial 
complexity in the size of the state space. Of course, the size of the state space is 
typically exponential in the degrees-of-freedom, or dimension, of the state space. 

Things become more complicated when there is uncertainty. But, there is a clas- 
sic information reduction which places the problem with uncertainty into the path 
planning problem presented above [Bertsekas, 1976]. With uncertainty everything 
becomes set valued. The information vector at stage j is the set of all observed 
measurements and applied controls up to time j 

^ = {yiX} 
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where the notation y^ represents the hrst through j th measurement, and similarly 
for the control. This vector represents all the measurement and control information 
available to the computer up to time j. In addition, there is prior information. 

The interpretation set is the set of all states that are consistent with the information 
vector and the prior information. There is a recursive mapping between the informa- 
tion vector and the interpretation set. Let /Co be a subset of X . This set represents 
th prior information about the initial state. A function A4(y), the measurement 
interpretation junction^ returns the set of all states that are consistent with the mea- 
surement y. The forward projection operator now acts on sets. The operator takes 
an input set in X and returns the set of possible outcomes given the uncertainty in 
u(x). The recursion for the interpretation set is then defined by 

/c J = ^(/c J _ 1 ,u(/c J _ 1 ))n^(y,)- 



Now the backchaining solution to the planning problem proceeds over the interpre- 
tation sets instead of X . In this model of the problem, the computational problem 
is order 0(k csad(U)2 card ^ x > ). The difficulty is exponential in the cardinality of the 
state. 

The output of the planner is a set of subgoal regions Qi and an associated action such 
that, given knowledge of the current subgoal, the robot can execute the sequence 
of actions and be guaranteed success. A subgoal is said to be recognizable if upon 
execution the sequence of actions and measurements which place the robot in Qi will 
generate an interpretation set which is contained in Qi. A recognizable subgoal is 
called a preimage. 

Heuristically, the difficulty of the full problem can be seen in the indexing of the 
interpretation sets. The interpretation sets run forward in time, whereas the planning 
solution must run backward in time. This means, in the worst case, in order to 
determine what to do on the next step, we have to know what we did for all previous 
steps and what we might measure at every step. 

For even the simplest of realistic problems, the complexity of the procedure is too 
great. In order to make the problem tractable, the requirements, the assumptions, 
the basis for the state space, or the search algorithm must be changed. This has 
produced several areas of work: sensorless manipulation, probabilistic manipulation, 
planning with feedback, reactive behaviors, and landmark based planning. Finally 
after discussing these approaches we discuss a new approach, Local Control about a 
Nominal Plan (LCNP), [Narasimhan, 1994], to which this thesis directly relates. 
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2.1.5 Sensorless Manipulation 



The constraints imposed by contact can be used to decrease uncertainty. For ex- 
ample, polygonal parts can be oriented by pushing on them with a stick. An early 
proponent of this approach was Mason in [Mason and Salisbury, 1985]. Only certain 
orientations will be stable, and the rest will be filtered out by the act of pushing. 
This is an example of sensorless manipulation. Sensorless manipulation relies on 
the task physics to accomplish the task without any sensing except for a clock. Al- 
though the direct approach to sensorless manipulation planning is still intractable, 
more powerful representations have made some problems tractable. 

One technique for simplifying the problem, which is also important in sensor based 
manipulation, is to compute the effect of an action and show that it has only a 
finite number of outcomes. The problem is further simplified by showing that the 
continuous set of actions fall into a finite number of action classes based on which 
of the finite outcomes they allow. For example, a tray tilting planner was devised 
in [Erdmann and Mason, 1988] based on the observation that a polygonal object 
could only come to rest in a finite number of orientations when it fell against a wall. 
An action sequence can then be derived by considering the action to be a sequence 
of filtering operations each step of which removes some of the unwanted possible 
orientations. 

This idea is very important for parts orienting and hxturing in manufacturing using 
vibratory bowl feeders or other similar systems. A good review of this work, and 
applications of using the constraints of shape to filter out and design the required 
function can be found in [Caine, 1993]. 

This approach can be placed in the preimage framework. Since the manipulations 
are sensorless, the information vector is just X 3 = {uj} and the interpretation update 
is 

K,j = J r (/C J _i,u(/C J _i)). 

Planning still proceeds over the power set of the state space. However, in this case 
since only a small number of states are stable, the dimensionality of the problem is 
tractable. 

A close dual to the sensorless manipulation problem can be found in [Brock, 1993]. 
Brock considers the problem of determining the configuration of an object through 
a sequence of sensing operations. The goal is to decrease the interpretation set until 
it is small enough to fit within a given goal set. 
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Within the preimage framework, he considered the class of manipulation strategies 
where there is very little uncertainty in the robot configuration and control relative 
to the uncertainty in the location of the object in the environment. In this case, it 
is possible to plan a sequence of motions which are guaranteed to reduce the object 
location uncertainty with every step. 

The state space is the combination of the robot's configuration and the object con- 
figuration. Since the error in locating the robot's configuration is very small, the 
analysis focuses purely on the interpretation set for the object. Finally, since the 
object is assumed to be fixed, the forward projection operation for the object is 
the identity operation. Therefore, in the preimage framework the interpretation set 
update after every measurement is 

where Mijfj) are the possible configurations of the object consistent with the new 
measurement. 

Every update step causes the interpretation set to either remain the same or decrease, 
since the update step is a sequence of intersection operations. The interpretation set 
can be made to strictly decrease by always moving the robot into the previous inter- 
pretation set. This will eventually shrink the interpretation set until it is sufficiently 
small to lie in the goal set expressed relative to the object configuration. A proba- 
bilistic viewpoint on this idea is given in [Hager, 1992]. 



2.1.6 Probabilistic or Randomized Strategies 



Probabilistic or randomized strategies can help the planning and manipulation prob- 
lem by: I) removing the requirement that every step in the plan be guaranteed to be 
recognizable, and 2) preventing infinite control looping caused by model or control 
error [Erdmann, 1989]. 

At the simplest level, a randomized strategy is just a random walk on the state 
space of the problem. If the state space is closed, bounded, and free of trap states, 
eventually, although slowly, the random walk will come within epsilon of every point 
in the space, including the desired goal. By biasing the search in the desired direction, 
using a measure of progress, convergence times can be significantly decreased. 

An everyday example of biased randomization is inserting a key in a tight lock. A 
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constant force in approximately the right direction is applied to the key and then 
the key is jiggled about rapidly to randomly break constraints and make progress 
against the friction. 

Biased randomization can be seen as an example of randomization with simple feed- 
back. These are strategies that only consider the current sensor value in deciding 
the next action to execute. The progress measure can be used to guide the selection 
of the next action. The resulting set of motions can be analyzed in terms of the 
expected progress to the goal. If, on average, the feedback controller causes motion 
towards the goal, the goal will be achieved asymptotically with probability 1, and 
the expected time to completion will be bounded. However, it can be very difficult 
to show that on average the motion does result in progress. 

Another advantage of randomization is that it tends to even out incorrect assump- 
tions about the environment. If a fixed plan fails in a certain configuration because 
of modeling errors than it will always fail in that configuration. However, with 
randomization a plan may succeed at this configuration by chance. 



2.1.7 Changing the State Space and Landmark Approaches 



An approach to simplifying the problem, which has appeared in many forms, is to 
change the representation of the state space. This representation should have two 
properties: 1) the representation should reduce the infinite, continuous, state space 
to a finite representation, and 2) the sensors should be able to separate the elements 
of the representation at least probabilistically. 

Primitive contact pairings between objects in the environment were defined as atoms 
in [Buckley, 1987]. This same representation was used in [McCarragher and Asada, 
1993a]. In order to incorporate the notion of sensor separation, Buckley defined two 
atoms as being disambiguous if the sensors (force and position) could distinguish the 
two atoms with a single reading from the sensors. Information states can then be 
defined as collections of atoms which are not disambiguous. These collections were 
termed distinguished sets. 

In the preimage framework, the state space is defined as the union of a disjoint 
collection of sets X = {Ji{Ai}. The information space is then the power set of 
A = {Ai}. Forward projection, backward projection, and preimages can then be 
defined relative to A. 
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Unfortunately, this notion of distinguished sets is too strong. In Cartesian config- 
uration spaces, force cones do provide a notion of distinguished sets. However, in 
problems with rotations the distinguished sets can be very large collections of the 
atoms. [Robles, 1995] discusses this point; this thesis will revisit this problem in 
chapter 5 when features are formally defined. 

A second difficulty is that atoms can be too large. Each atom represents some region 
of configuration space. In Buckley's approach the forward projection operation is 
applied to the entire atom to see if the next desired atom can be reached. Buckley 
shows that it is easy to construct examples where only a subset of the first atom can 
achieve the next goal. Both difficulties can be partially circumvented by limiting the 
state space by supplying a nominal path. 

Another approach to creating a finite set on which to reason is landmark based 
planning as presented in [Lazanas and Latombe, 1992]. This paper shows that the 
original motion planning problem of finding a guaranteed plan despite uncertainty 
can be solved in polynomial time if state space is inhabited by regions called land- 
marks and the goal in one of the landmarks. Within each landmark the robot is able 
to navigate and know exactly what its state is. Between landmarks the robot moves 
with uncertain control and no sensing. 



2.1.8 Behavior Approaches 



A behavior based approach to robot task control evolved as an alternative to planning 
to try and circumvent the problems of computational complexity. Behaviors are a 
collection of feedback loops connecting sensor measurements to actions [Brooks, 1987, 
Brooks, 1991]. 

Behavior based approaches are important because they change the framework of 
the planning and design problem. Whereas before the planning problem was con- 
sidered to be the determination of a set of command sequences, which might be 
motion primitives, for every sensor measurement sequence and action sequence, the 
behavior paradigm is to determine a set of feedback loop primitives that will be 
applied in different contexts. For example, in the peg-in-hole problem, the planner 
would look for a sequence of velocity commands to apply to a single admittance 
controller. The behavior approach might use a set of admittance controllers and 
surface following controllers to achieve the task. The idea of increasing the power of 
the admittance controller appears in [Schimmels and Peshkin, 1993]. The behavior 
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approach attempts to exploit the error reducing property of feedback to make the 
general problem more tractable. 

Although the approach is promising, there are two basic problems that remain to be 
solved, and which must be solved in a domain specific way. The first is the devel- 
opment of the feedback controllers. One approach is to develop feedback controllers 
which achieve nominal progress in the absence of sensor uncertainty. These can be 
developed through learning over simulated data, or directly from models of the task 
physics. 

Second, once a set of controllers is developed, the domain of each controller must 
be determined. For feedback controllers, without information state, the domain 
is the region of state space over which the controller should be applied. In the 
subsumption architecture, the domain is arbitrated through behavior suppression. 
[Mataric, 1994] used reinforcement learning to determine the suppression rules given 
a set of real sensors. This set of rules attempts to approximate the optimal regions 
of applicability given the limitations of the sensors. [Erdmann, 1993] showed that 
backchaining planning, assuming perfect control, implicitly defines the regions of 
applicability for the controllers. He then discussed the problem of designing a sensor 
which can, with a single measurement, return the current region and an appropriate 
sensed value. 



2.2 Feature Based Programming 



This historical review suggests the following about a complete solution to the ma- 
nipulation programming problem: 

• global knowledge of the geometry is required; 

• randomization should be used and expected; 

• feedback is necessary; 

• the feedback law should depend upon the local context; and 

• the local context should come from a finite graph representation of the state 
space, where the representation is derived from the sensors capabilities. 

An approach described in [Narasimhan, 1994] synthesizes these ideas into an ap- 
proach which uses a set of indexed local controllers to control the motion along a 
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nominal plan. He terms this idea Local Control about a Nominal Plan (LCNP). The 
idea is to plan a path through the state space which will accomplish the goal as if 
there were no uncertainty. This problem has polynomial complexity in the size of the 
state space. Then a set of feedback controllers is learned, from simulated examples, 
which keeps the robot on the desired path. As in classical feedback control, the 
controller computes a command to the actuators as if its sensor inputs were exact. 
This approach is sometimes called certainty equivalent control. 

A controller is learned for each local context by inverting the experimentally learned 
input-output relationship between small motions of the robot and motions of the 
part. For the task of pushing, the local context is a discrete representation of the 
local configuration space surface. In operation, each sensor value returns the local 
context and an estimate of the robot's location. The local context is then used 
to determine the appropriate feedback controller, and the robot's location and the 
desired path are used to determine the next control action. The result is a diffusion 
process which on average makes progress to the goal. 

The path can also be iteratively modified to increase the expected performance of the 
plan. Modification can be seen as a form of the expectation modification algorithm 
(EM). After the initial seed path is generated, assuming no uncertainty, the algorithm 
iteratively simulates possible outcomes and modifies the path in order to determine 
a more robust strategy. 

A similar idea is presented in [Robles, 1995], although here the focus is assembly. In 
this thesis, an action map for states that might be visited given the nominal path 
is determined using backchain planning without uncertainty. Upon execution the 
controller iteratively computes the interpretation set and selects an action from the 
states in the current interpretation set. The interpretation set is represented as a list 
of bounding polyhedra in the configuration space. The select operator may or may 
not involve randomness. 

When the decisions about the current contact state are made on the basis of local 
signal models, the approach can be termed feature based programming. A feature 
is a model of the measurement process that applies to a local patch of state space. 
By determining which model best explains the measurements, the robot controller 
can determine approximately where it is in state space. If the features select the 
controller, then determining the current feature is sufficient for determining the form 
of the local controller for each patch of state-space. 
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Figure 2.2: The components of the planning and control system for 
the feature approach to robot control. This thesis considers the shaded 
components. 



There are three components to the feature approach: 1) the nominal plan, 2) the local 
controllers, and 3) the feature observer. As in the LCNP approach, the nominal plan 
is constructed assuming no uncertainty. The local controllers can be learned from 
real or simulated examples. This thesis focuses on the feature observer component 
of this approach. The observer is designed to determine the probabilities of each 
feature model. The probability distribution over the features is the context which 
can be used for selecting the local controller. The components of the planning and 
control system for the feature approach are illustrated in figure 2.2. 



In this thesis, a statistical modeling approach is adopted for modeling the force and 
position features. This eliminates the need for setting thresholds for different prob- 
lems. Instead the system automatically calibrates itself to the noise conditions and 
detects changes which are statistically relevant. Second, the statistical viewpoint 
allows the derivation of different levels of feature sensing within a single decision 
making framework. Lower levels can be used independently for simpler robot ma- 
nipulation problems. Furthermore, these feature detectors isolate the raw force and 
velocity signal from the geometric part of the observer. Only changes in feature vec- 
tors need to be processed by the geometric part of the observer in order to determine 
the new contact state. This significantly reduces the computational load. 
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The problem of building an observer based on force measurements was also considered 
in [McCarragher and Asada, 1993a]. They used a Petri net to model the different 
contact conditions and possible transitions for known geometry. Transitions were 
detected using transient contact force features. The approach did not check the 
consistency of the forces with the current contact state. Models for the effect of 
Petri net transitions on the force were derived through qualitative reasoning on the 
dynamics. Using this technique, they were able to insert planar pegs into a planar 
hole. 

Before focusing on the features used in this thesis, it is useful to consider the range 
of features that are practically observable in manipulation. The next section hrst 
considers a possible framework for the types of cues available to humans. Then, the 
subset of cues available to a robot which is only able to sense position, force, and 
torque on the contacting link is discussed. These are the only measurements available 
in the mating phase of manipulation. Therefore, an increased understanding of just 
these signals would significantly enhance a basic component of manipulation. Finally, 
previous work in this area of sensing called intrinsic tactile sensing is presented to 
relate the features used in the thesis to prior work. 



Contact Manipulation Cues 



Chapter 3 



Manipulation sensing is a complex task requiring the interpretation of many different 
types of measurements. It is useful to consider the cues used in human manipula- 
tion and then to consider what subset of these cues is available to a robot which 
is equipped with only a force/torque and position sensor. These two sensors pro- 
vide the measurement equivalent of tool based manipulation, with which humans 
have great perceptual facility. It is also the only source of contact measurement 
information available in mating tasks. A better representation and understanding of 
this information should make it possible to give robots the same level of perceptual 
capability. 

This chapter provides a general overview of human manipulation cues, and then 
discusses the cues available in mating tasks. The processing of contact information 
without direct contact sensing is termed intrinsic contact sensing. The relationship 
of previous work in this area to this thesis is discussed. Finally, the hardware used 
in this thesis is presented. 

3.1 Human Manipulation Cues 



In general, touch tasks can be broadly divided into exploration tasks and manipula- 
tion tasks [Srinivasan, 1991]. Exploration tasks are sensor dominant, which means 
that they depend primarily on sensory inputs for successful completion. These tasks 
typically involve discrimination or identification of surface properties (such as shape 
and surface texture) and volumetric properties (such as mass and compliance) of an 
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object. Manipulation tasks are motor dominant, depending primarily on voluntary 
motor activity to modify the environment, although sensory feedback is essential for 
successful performance. In a motor dominant task, sensory feedback provides cues 
about the state of the contact configuration both between the hand and the grasped 
object, and between the object and the rest of the world. Manipulation tasks can 
be further subdivided into precision or dexterous tasks and power tasks. Dexterous 
tasks are performed primarily with the fingertips, while power tasks are performed 
with the entire hand. 

Exploratory tasks involve so called identification cues which can be passively or 
actively acquired. Passively acquired identification cues can be obtained by the act 
of holding the object in the hand. Three major cues in this category are: 

• Determining the local contact normal and curvature of the surface at each con- 
tact. Determining the local contact curvature and labeling it as a point, edge, or 
planar contact is important for grasp acquisition and object identification. Dif- 
ferent contact types have different grasp properties which affects stability [Sal- 
isbury, 1982]. In addition, the local contact normal and curvature provide a 
strong pruning heuristic rule for identifying objects and object pose [Grimson, 
1990]. Local object curvature can be measured passively by examining the 
normal contact force distribution. 

• Determining the surface texture at each contact. Surface texture affects grasp 
stability. Rough textures are generally easier to grasp then smooth textures. In 
addition, surface texture can also be used as a pruning heuristic in identifying 
objects and object pose. Texture cues are produced both from the spatial 
distribution of the contact force and the dynamic vibration effects produced at 
the skin during motion. 

• Determining the gross object shape. By using the finger geometry and the 
location of the contact points in the hand, the shape and pose of known objects 
can be estimated [Siegel, 1991] . For example, an initial grasp might be a power 
grasp in order to get a lot of contact information about an object. Once 
the object and its shape is identified, a dexterous grasp might be used for 
manipulation. 



With some local active exploration, the following properties relating the reaction 
forces to the applied force can also be determined: 



• The local stiffness at each contact. The local surface stiffness can be estimated 
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by applying varying normal forces and measuring the change in contact area. 
Softer surfaces will produce a larger contact area for a given applied force. The 
local stiffness of an object can also be estimated by tapping with a probe and 
looking at the initial component of the resulting impact signature. 

• The local frictional properties. The local friction along with the local contact 
type strongly effects grasp stability. Friction can be measured by applying 
varying tangential forces and then detecting the onset of slip. 



In addition to identification cues there are manipulation cues. Manipulation cues 
are interpretations of contact events that occur only during manipulation. The cues 
require relative motion between the hand and the object, or between a grasped object 
and the environment. Some of the basic cues are: 



• Detecting slip. Detecting the onset of slip between an object and the hand 
is essential for grasp maintenance. Slip detection is used to determine the 
necessary grasp forces at each contact during all stages of manipulation. 

• Determining object mass, center of mass, and moments of inertia. 

By manipulating a grasped object and measuring the resulting joint torques and 
net contact forces at the hands for different configurations, the mass, center of 
mass, and moments of inertia of the object can be computed. This information 
can be used for object identification and pose determination [Siegel, 1991], as 
well as for computing the necessary torques for throwing or manipulating the 
object. 

• Estimating directions and type of contact constraint. Assembly is the process 
of bringing a moving part into a constrained relationship with a fixed part. 
In order to control the assembly process, the directions in which movement 
is constrained need to be estimated. Contact constraints are estimated using 
measurements of the reaction forces and allowed velocities as a function of 
position, and by measuring the direction of impact forces. 

• Detecting changes in contact constraints. This is one of the most common cues 
during manipulation. The detents in switches, the termination in screws, the 
impacts from mating two parts are all examples. The onset of the change can 
be detected by looking for impact forces. The direction of the impact force 
provides some information about the new constraint. 
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3.2 Intrinsic Contact Cues 



In mating or contact sensing through a tool the robot is only able to sense the 
generalized position and force on the contacting part or link. 1 This is only a small 
subset of the cues available in manipulation sensing. 

The robot may or may not know the pose (relative to itself) and geometry of the 
contacted part. If the geometry and pose are known, the type of sensing is termed 
intrinsic contact sensing. For example, this is the case when a force sensor is attached 
with a known configuration and geometry to a robot. Knowledge of the geometry 
and pose of the part makes it possible to determine the position of the contact on the 
part under the assumption of a single point region [Salisbury, 1984]. With a convex 
sensor, and the assumption of a single point contact, the local contact normal and 
the object shape can be constructed through probing. 

Without knowledge of the geometry and/or the pose, only the temporal and cross- 
correlation structure of the measurements is available to the robot. The robot needs 
additional prior knowledge in order to relate this abstract measurement information 
to more general manipulation cues. For example, if the robot knows that a temporal 
signal is the result of stroking a texture, then different textures can be sorted based 
on temporal effects. Similarly, the additional vibration induced by slip can be used 
as an indicator of slip, if the robot knows that slip is the only possible source of 
additional vibration. Techniques for both of these measurements are considered in 
chapter 7. 

Constraint and changes in constraint can be directly measured by the robot. Tech- 
niques for estimating the constraints for certain classes of environments are consid- 
ered in chapter 8. Determining the object mass, center of mass, and moments of 
inertia is a direct application of estimation techniques in robot calibration and adap- 
tive control. Not all of these terms are directly observable, but certain combinations 
can be determined using recursive estimation techniques [Slotine and Li, 1987]. 



^he generalized position is the set of parameters need to specify the position and orientation 
of an object. This is also termed the object's configuration. The generalized force is the force or 
torque on each of the generalized directions. 
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3.3 Previous Work on Intrinsic Contact Sensing 



During the last decade, considerable research has been performed on tactile sensing. 
[Howe and Cutkosky, 1991] provides a recent comprehensive review of current and 
past research. Most of this research has focused on designing surface array sensors 
and using these sensors for obtaining geometric information from static measure- 
ments. Some research has looked at the information that can be acquired by actively 
moving the contact sensor and monitoring both the sensor and joint locations. This 
is termed haptic sensing. Prior haptic research has primarily focused on actively 
tracing the contours of objects to determine geometry and critical features [Brock 
and Chiu, 1985, Stansheld, 1987]. This work assumes that each measurement is 
taken with the force sensor in a quasi-static state so that normal forces and contact 
locations can be computed. All of this work essentially treats sensing with a tactile 
array sensor as a primitive form of vision. 

In contrast, only recently have investigations examined contact information that 
is characteristic of the physics of motion [Howe and Cutkosky, 1991]. Mechanical 
properties of objects like mass, friction, and damping can only be determined by 
actively probing and manipulating the object. Similarly, detecting the initial contact 
with an object, and slip between the sensors and environment require sensing the 
effects of relative motion. 

A few studies have been done on this type of sensing. By monitoring the acoustic 
emission from a metal gripper, [Dornfeld and Handy, 1987] detected the onset of 
slip for some metallic workpieces. [Howe and Cutkosky, 1989] constructed an instru- 
mented latex covered finger. Piezoelectric sensors are embedded in the latex cover 
and a miniature accelerometer is mounted on the inside surface of the cover. The 
piezoelectric sensors are very sensitive to strain rate. Because of the small mass of 
the cover, the accelerometers are sensitive to very small forces normal to the surface 
of the sensor. They found that the piezoelectric sensor was very sensitive to the 
changes in tangential strain associated with slip, and that the accelerometer was 
fairly sensitive to the vibrations normal to the sensor associated with slip. 

Determining the contact location, given the sensor's geometry and the assumption 
of a single contact has received considerable attention since the original paper [Salis- 
bury, 1984]. Additional extensions have appeared in [Bicchi, 1990, Bicchi et al., 1990, 
Eberman and Salisbury, 1989, Bicchi et al. } 1989, Bicchi et al., 1989, Gordon and 
Townsend, 1989, Kanekp and Tanie, 1992]. Most of these papers discuss how the 
contact location for a single point contact can be determined from joint torque or 
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Figure 3.1: 6-axis fingertip force-torque sensor 

internal wrench measurements and a model of the geometry. [Bicchi et al., 1993] 
presents a general theory for this problem. 

A six-axis fingertip force-torque sensor was used in [Bicchi et al., 1989] to estimate 
the onset of slip (figure 3.1). This sensor has a Maltese-cross connecting the outer 
shell to the base. The cross is instrumented with 8 strain-gauge half-bridges. The 
shell has a lightly damped natural frequency of approximately 690 Hz when the base 
is fixed and the shell free. In his experiments, Bicchi hrst determined the coefficient of 
friction for the object to be grasped. Then, by monitoring the ratio of the tangential 
force to the normal force, he was able to determine when the contact state was 
approaching the slip condition determined earlier. 

This thesis extends this work of extracting primitive features to include temporal 
features and constraint features. Furthermore, the feature primitives are placed into 
a single framework which can be used for manipulation. In an earlier paper [Eberman 
and Salisbury, 1993], we showed how intrinsic contact sensing could be used to detect 
changes in the spectral signal characteristics of the contact wrench. Greater detail 
on this approach is contained in [Eberman and Salisbury, 1994]. Chapter 7 covers 
some aspects of this work. 



Although constraint is an important part of the assembly process, and is critical to 
force control, there appears to be little work in active sensing or processing of the 
information for the purposes of identifying the constraints on a part. [Simunovic, 
1979] presents one of the hrst results in this area. This paper considered determining 
the relative position between the grasped part and the mating part using a sequence 
of position measurements assuming all measurements came from a single known 
contact configuration. He formulates an Extended Kalman Filter for the unknown 
positional bias between two parts. The filter takes in position measurements and 
recursively estimates the bias. His suggestions for future work anticipate part of the 
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work in this thesis. To quote from his thesis: 

The implementation of the methods in this work requires further devel- 
opment in the following areas: 



• develop the necessary checks and implementation to allow for par- 
allel estimation and identification of alternative touching configura- 
tions; 



More recently [Delson, 1994] looked at programming a robot using human demon- 
stration. As part of this work he developed some tests for segmenting the motion 
data into section that come from a single contact configuration. The tests were done 
either only on the force or on the velocity depending upon the contact condition. 
Because this is the focus of this thesis, this thesis significantly extends his work 
by explicitly modeling the process noise, making decisions on all of the data, mak- 
ing all the decisions on both the position and the force information, and creating a 
framework that can incorporate more than just constraint models. 



3.4 Hardware 



This thesis used a 6-axis force torque sensor, used by Bicchi, that is designed for 
the Salisbury Hand (figure 3.1) for all the experiments. The sensor is attached to 
a number of different experimental apparatus. The hrst device was a single glass 
epoxy link which was driven by a torque controlled brushless motor (figure 3.2). 
Under closed-loop torque control, this system is able to control the forces to 1 part 
in 500 with very little internal vibration. The motor's design and control is described 
in [Levin, 1990]. 

The second device mounted the fingertip sensor on the PHANToM. The PHANToM 
is a three degree-of-freedom force reflecting haptic interface. The design gives it a 
high force dynamic range (around 100:1) and low vibration levels during motion. In 
this thesis it was used as a large finger (figure 1.2). 
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Figure 3.2: Single degree-of-freedom hardware 



3.5 Conclusion 



Force and torque information combined with velocity information provides the sens- 
ing equivalent of human tool manipulation. Although this is only a small subset of 
the information used during human manipulation, it is one of the most common and 
important forms of environmental interaction. Only recently has the tactile sensing 
community begun to work in this area. Previous work in this area has tended to 
focus on grasping issues. 



The next chapter presents configuration space as necessary background material for 
the remaining thesis presentation. 



Manipulation and Constraints 



Chapter 4 



This chapter discusses a representation for the motions of and forces on objects with 
contact constraint. The forces that the force sensor measures are caused by reaction 
forces from the constraints and by relative motion along the constraints. Additional 
background on this chapter's treatment of configuration space and deterministic 
dynamics can be found in books on advanced classical mechanics. A very readable 
treatment of the methods of mathematical classical mechanics including manifolds, 
tangent bundles, cotangent bundles, and forms from a physics viewpoint is given 
by [Arnold, 1989]. An introductory mathematics text on this material is [Munkres, 
1991]. 

The structure of the dynamics and the sensor measurements in configuration space 
creates a natural framework in which to construct a feature observer. Motions in 
configuration space give rise to contact models, namely: 1) impacts, 2) constraints, 
and 3) velocity dependent characteristics of the noise. An observer can be designed 
to work with these models in order to estimate the current measurement model. 
The current model determines a set of configurations, corresponding to a piece of 
configuration space topology that the robot might currently occupy. These pieces 
of topology are connected in the configuration space and induce notions of connec- 
tivity between the measurement models. The next chapter formalizes this idea and 
discusses possible definitions for connectivity between the models. 

This chapter provides background for the other chapters and can be skimmed and 
then referred to when reading the other chapters. The essential ideas to get from 
this chapter are: 
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1. The representation of motion in terms of configuration space. 

2. That configuration space consists of the union of smooth surfaces. 

3. That each surface has a collection of tangent surfaces, called the tangent bundle, 
and that each surface also has a collection of generalized normals called the 
cotangent bundle. 

4. That contact forces are generated by contact with these surfaces. 

5. That impacts occur because of transitions between these surfaces. 



Finally, the end of this chapter provides a statistical treatment of dynamics and force 
sensing for a robot. This section is not central to the main thread of the thesis. 



4.1 Configuration Space 



Manipulation is the problem of changing the location and orientation, or configu- 
ration, of objects in the environment in order to bring objects into contact. Every 
contact on an object presents constraints on the allowed motions of the object. As 
the object moves the contacts and therefore the constraints on an object change 
discontinuously. Configuration space makes these kinematic constraints explicit. 

The position of a body relative to a fixed origin is defined by a set of generalized 
coordinates x. The set of all allowed coordinates is called the configuration space 
denoted by C. For example, the configuration of a point in space can be specified 
by a vector giving the Cartesian coordinate values for the location of the point. The 
configuration of an jointed serial linkage can be specified by its n joint coordinates. 
The configuration of a polygon in the plane can be specified by the position and 
orientation of its coordinate frame. 

Configuration space has many important properties. First, obstacles in the environ- 
ment can be mapped into constraint surfaces in the configuration space. Motion into 
these surfaces is prevented by a configuration space constraint force which we term 
a constraint wrench. The total contact wrench is the sum of the constraint wrenches 
and the wrenches tangent to the constraints. The sum all of all the contact wrenches 
is what is measured by the intrinsic contact sensor. 1 



^^Wrench and twist typically refer to the generalized force and velocity of a three dimensional 
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When the accelerations of the body can be neglected all of the bodies dynamics 
can be represented in configuration space. The quasi-static assumption is that the 
net wrench on the body is zero. An analysis, given in this chapter, shows that this 
assumption is equivalent to a stationary statistical assumption for the second order 
dynamics. For the majority of robot interactions this assumption is enforced by using 
an appropriate control algorithm. Section 4.3 discusses this assumption in greater 
detail. With the quasi-static assumption, the generalized velocity of the body, or 
twist, is a function only of the desired motion and the configuration. 



4.2 Kinematic Constraints 



Since the position of the body is represented by a point in C, constraints created by 
contact between objects are transformed to constraints on the motion of the point. 
The point in C which correspond to the overlap of objects are called configuration 
space obstacles denoted by O. The complement of the obstacles space is the free 
space and is denoted by T . This section discusses the characteristics of these kine- 
matic constraints in general. The computation of the constraints for polygons in the 
plane and points in Cartesian space is deferred to chapter 8. As an example of the 
concepts, we discuss the configuration space for a moving polygon on a plane with 
other polygons as obstacles. 

Most man-made objects can be modeled as the union of several generalized surfaces. 
For example, a circle has one surface: the outer edge. A triangle has six surfaces: 
three edges and three vertices. These are different surfaces because there is a discon- 
tinuity in the triangle at each corner. The dimension of a surface is the number of 
coordinates it takes to parametrically specify a location on the surface. In general, 
objects can be broken down into surfaces of equal dimension by separating the object 
along places of geometric discontinuity. Each of these surfaces is called a manifold. 
This separation can be extended to include changes in surface properties like texture 
and friction. In this section, each surface of an object is assumed to have uniform 
texture and friction. 

The geometric form of the manifold depends upon some geometric parameters §i £ 
9£ fc . For example, the location of a vertex is specified by its coordinates in the object 
frame. The location of an edge can be given by the outward normal of the line 



body when these terms are represented as screws [McCarthy, 1990]. We are using these terms for 
any generalized force and velocity to avoid confusion with the force and velocity of a point mass. 
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supporting the edge and the distance to the edge. This last example shows that the 
boundaries of a manifold are not described directly by the geometric parameters. 
A boundary is defined by the intersection of two manifolds which is itself another 
manifold. 

The structure of objects as collections of manifolds induces the same type of structure 
on the configuration space obstacles. A single contact between a surface on the 
moving object and a surface in the environment can be defined by a constraint 
equation 

C(/ m ,/ e ) : ^ km x »*« x C - ». (4.1) 

C(j m j e ) is the constraint equation for contact between surface f m on the moving 
object, described by k m parameters, and surface / e , described by k e parameters, of 
the environment. 



The constraint manifolds define the boundary of the configuration space obstacles. 
For many object types, each of these surfaces can come from only a finite class 
of primitives. For polygons each surface must be either a vertex or an edge. For 
polyhedra the surface must be either a vertex, an edge, or a face. Therefore, a 
constraint equation can be considered to be of a certain type 

C type : 9£ fci x £*> xC^i (4.2) 

The set of x which satisfy C type (gi } g J7 x) = defines a constraint manifold A4ij for 
feature pair (i,j). Note that the free space is itself a manifold. 

The configuration space manifolds for a point robot in 3J 3 are easy to visualize. The 
configuration space for a point robot is just 3J 3 . The configuration space constraints 
are the surfaces that the point can contact in the original space. For example, if the 
point is touching a plane, then that plane creates a plane constraint manifold in the 
configuration space. 

The configuration space of a polygon in a plane is the space of coordinate transfor- 
mations 3J 2 X SO{2). S 0(2) is the two dimensional rotation group which is param- 
eterized by a single value 6. Therefore, the configuration space is three dimensional. 
Therefore, transformations can be specified by the triple (x } y } 6) which makes visu- 
alization easier. 

For polygons there are two basic constraint types. Type A is contact between an 
edge on the moving polygon and a vertex on the fixed polygon. Type B contact is 
contact between a vertex on the moving polygon and an edge of the fixed polygon ( 
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Figure 4.1: Type B (vertex-edge) and type A (edge- vertex) contact 
between two polygons. Figure from [Caine 1993]. 



[Lozano-Perez, 1983]). Contact between two vertices is a limiting case of contacting 
two edges. Contact between two edges can also be treated as two contacts. Both 
cases will be considered in the next section. 

Figure 4.1 illustrates the configuration space obstacles for type A and B contacts. 
Because polygons consist of straight lines, the configuration space obstacles are ruled 
surfaces. At any angle 6 the configuration space obstacle is a straight line. The 
orientation of this line changes as the orientation of the moving object changes. 



The curvature of the surface is a function of the signed distance of the moving origin 
from the contact. The curvature of the surface can be changed by moving the control 
point. The surface can be changed from convex to concave by moving the control 
point to the other side of the contacted edge. 



56 



Chapter J f : Manipulation and Constraints 




Figure 4.2: Two type B contacts creating a corner [Caine 1993]. 



4.2.1 Multiple Contacts 



Multiple contacts are specified by requiring several constraint equations to hold si- 
multaneously. With m constraint equations the complete constraint equation can be 
written as 

3T 1 (4.3) 



(~1 • S&kgroup v f 



where k group is the vector of parameters specifying the contact geometry. The set of 
all x which satisfy C group (g group} x) = is again a manifold A4 group . This surface is 
the intersection of all the A4ij for each pair (i,j) in the constraint group. 

Just like with the underlying objects, the intersection of two configuration space man- 
ifolds creates a manifold of lower dimension. Two 2 dimensional surfaces intersect 
to create a one dimensional curve. Two curves intersect to create a point. 



Figure 4.2 shows the configuration space obstacle for two vertices contacting a corner. 
The obstacle takes the form of a section of a helix. The obstacle is the intersection 
of the two constraints imposed by the contact between the two vertices and the two 
edges. 
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4.2.2 Connectivity of Configuration Space 



Figure 4.3 shows the configuration space for a triangular object with a single quadri- 
lateral obstacle. There are several important properties shown in this configuration 
obstacle. First, the complete configuration space consists of the union of many 
smooth manifolds. Each manifold surface is generated from one particular pairing of 
features on the moving object and features on the fixed object. The connectivity of 
these manifolds forms a natural graph of the topology of the configuration space. 

There are two important types of connectivity for the configuration space topology. 
Path connectivity labels two manifolds as connected if there is a path connecting the 
manifolds which lies entirely in the two manifolds. Phase flow connectivity refines 
path connectivity be requiring that the path be an integral curve of the dynamics. 
That is the path must be physically realizable given the dynamics and any kinematic 
constraints. 

Figure 4.4 shows a graph representation of the topology for the configuration space 
for the indicated objects . Each node in the graph has been labeled by the featuring 
pairings that are active at that graph node. This graph has been called the graph 
of assembly process states or the contact states in [Asada and Hirai, 1989]. For 
non-convex objects, such as peg-in-hole assembly problems, the connectivity is sig- 
nificantly more complicated than the graph shown. In either case, the graph shows 
the possible contact transitions and is invariant to changes in the reference point. 

Phase flow connectivity represents the possible connections given the dynamics. Non- 
holonomic systems have differential constraints on the allowed velocities at any point 
in configuration space. A car can only move in the direction of the instant center 
formed by the wheels. In general, this is the important definition of topological con- 
nectivity. Contact conditions are connected if there is an integral curve that connects 
the two contacts. 

Figure 4.3 also illustrates that the surface of any individual manifold is smooth or 
differentiable. 2 The surface is smooth since each manifold is defined by the set of 
allowed configurations for pairings between smoothly varying features on the mov- 
ing and constraint objects. Therefore, the wrenches of constraint and the allowed 
motions on the surface do not change abruptly, and the second order dynamics of 
motion for the moving object is continuous for motions constrained to lie on a single 



2 Note that the straight lines in the figure are artificial and only serve to illustrate the curvature 
of the surface. 
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Figure 4.3: Triangular object with a single quadrilateral obstacle. The 
figure shows the configuration space with regions labeled with the fea- 
ture pairings. Note how edge-edge contacts are formed by the intersec- 
tion of the bounding vertex-edge contacts. (Figure from [Caine 1993]). 
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Figure 4.4: Path connectivity of the configuration space topology for 
the indicated convex objects. The configuration space is the (x } y } 6) 
coordinates of the triangle. Each dot in the matrix indicates a possible 
pairing between features on the triangle and features on the parallel- 
ogram. A segment indicates the pairings are connected. The other 
diagonal direction does not appear because those segments represent 
paths that cause objects to pass through each other. 



manifold. 

Therefore, the nodes of the topology graph represent connected, smooth regions of 
configuration space and edges represent discontinuous transitions between surfaces. 
As section 4.3 discusses, this structure implies the quasi-static assumption will be 
true on any one manifold, with dynamic effects restricted to the transitions given a 
smooth, stable controller. 



4.2.3 Tangent and Cotangent Bundles 



For every point on a constraint manifold A4, there are two associated vector spaces: 
the tangent space and the cotangent space. The tangent space is the vector space 
of all velocities which would results in motions that differentially remain on the 
constraint. The cotangent space is the dual vector space which is in the null space 
of the tangent space. The cotangent space is the space of all wrenches of constraint. 
The cotangent space is the space spanned by the derivative of the constraint equation 
at a point x. Again, the tangent space lies in the null space of the cotangent space. 
In the force control literature, the inner product of vectors in the tangent space and 
covectors in the cotangent space is termed the reciprocal product. This inner product 
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T*(M) 




Figure 4.5: The tangent T x (Ai) and cotangent T^(Ai) spaces at a point 
on a manifold. 



is always zero since the tangent space is the null space of the cotangent space. 

We denote the tangent space at a point x as T x (Ai) and the associated cotangent 
space as T^(Ai). The union of these spaces over all points on the manifold is called 
the tangent bundle and cotangent bundle. 



T(M) -- 


-- U MM) 




xeM 


T*(M) = 


~- U V(M) 



(4.4) 
(4.5) 



Figure 4.5 illustrates these ideas. The configuration space together with its tangent 
bundle is called the phase-space. 



When the constraint surfaces are produced through contact, and not through con- 
nection of a physical linkage, T^(Ai) is unisense because the constraint equations 
are properly inequalities. Constraint wrenches can only be produced to prevent the 
body from penetrating the obstacles. Constraint wrenches which prevent the body 
from leaving the obstacles are not possible. Our constraint estimation procedures 
will be interested in isolating only those times when the constraint is active. In this 
case, the constraint can be treated as an equality constraint. 
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Reduction to 
Configuration Space 

Figure 4.6: Configuration space manifold equivalence for a Phillips and 
flathead screw [Caine 1993]. 



4.2.4 Configuration Space Equivalence 



While the geometric parameters, g 3ro «p, of any constraint surface are sufficient to 
describe the surface, they may not be unique. Thus, the problem of determining 
equivalence between two different descriptions arises. Two descriptions g and g 
produce equivalent manifolds, or are equivalent, if they produce the same constraint 
surface. 

For example, a Phillips screw and screw driver produce the same constraint surfaces 
as a flathead screw and screwdriver for (z,6) motions (Figure 4.6). Several exam- 
ples of this are given in [Caine, 1993]. Some mechanisms also have this property. 
Figure 4.7 shows a moving triangle contacting the environment with two type B 
contacts. These contacts, except for the limited range of motion, are equivalent to a 
mechanism with two sliders as shown. Each contacting vertex can be considered as 
attached to a slider aligned with the edge through a pivot. An equivalent mechanism 
can be constructed by moving the attachment points and slider directions. Both the 
triangle and the indicated mechanism generate the configuration space constraint 
curve shown. 



Equivalence can be tested by examining the tangent bundles. Two descriptions g and 
g are equivalent if at every x£M the span of the cotangent and tangent spaces are 
the same. Clearly, two equivalent manifolds have the same tangent bundles because 
they are exactly the same surface. Two equivalent tangent bundles also determine 
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Configuration Space Constraint Curve 



Moving Object 




2 independent 
slider, rotary pairs 



Figure 4.7: Configuration space manifold equivalence for two different 
slider assemblies. 



the same surface because they have the same integral curves. 

The tangent bundle notion of equivalence is sometimes much easier to test. Two 
tangent bundles are equivalent if the functions which generate a basis for the tangent 
space span the same space at every point. For two constraints in a three degree-of- 
freedom problem, the span consists of only a single vector. Equivalence can be 
checked by looking to see if the two mechanisms produce the same vector, up to a 
change in scale, by looking at the functional form for the vector. This example case 
of two type B contacts is shown to be equivalent in section 8.1.3.1. 



4.3 Dynamics 



One of the difficult and challenging properties of manipulation is that it takes place in 
a dynamic environment where the dynamics switch discontinuously as a function of 
the generalized coordinates. Although this class of problem arises often in practice, 
a formal control treatment has not yet been developed for analyzing stability or 
robustness. 
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In this section we discuss the physics and statistics of manipulation for a robot The 
analysis starts by formulating the second order, Hamiltonian, stochastic equations 
of motion for a system consisting of a robot, an sensor, an end-effector, and the 
environment for a single contact condition. The Hamiltonian formulation uses the 
momentum and position as basic state variables. The effect of assuming statisti- 
cal stationarity on the state and measurement statistics is then determined. This 
analysis shows that the quasi-static assumption for manipulation is equivalent to the 
expected value of the stationary statistics of the second order dynamics. The analy- 
sis also shows how variation in the constraint equations creates a velocity dependent 
disturbance and how the other disturbances create errors in the wrench signal. 

Then transitions between constraints are considered. These transitions correspond 
to edge transitions in the topology graph. Transitions almost always create impact 
forces because of either: I) the discontinuous change in momentum required to satisfy 
the constraint, or 2) the discontinuous loss of constraint resulting in the release 
of stored elastic energy. Detecting and isolating these transitions is important in 
rigid body manipulation because they correspond to regions where the quasi-static 
assumption does not hold. Furthermore, the direction of momentum change provides 
information about which edge in the topology graph was the source of the transition. 



4.3.1 Constant Contact Constraints 



The equations of dynamics and the process statistics are most easily represented by 
considering the momentum and position form (Hamiltonian) of the dynamic equa- 
tions. This approach results in two sets of first order differential equations instead 
of a single set of second order differential equations. 

Figure 4.8 shows an example of contact between the end-effector (this could be an 
object held in a grasp) and the environment. The configuration of the robot and the 
end-effector is specified by x r and x respectively. They are connected by the sensor 
which is represented as a generalized spring K and damper B. They have inertia 
H r and H respectively. The generalized momenta of the robot and end-effector are 
p r = H r x r and p = Hx. The total momentum is p t = p r + p. We will assume that 
the generalized inertia matrix is always invertible. This implies that that robot is 
always away from singular configurations. 
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Robot 




Environment 



Figure 4.8: Model of the robot, the force/torque sensor, the environ- 
ment and the contacting link. 



The wrench measured by the sensor is 



w r 



K(x-x r ) + B(x 






(4.6) 



since both the stiffness and damping loads are measured by the strain gauges in the 
sensor. With this notation, the dynamics of the robot and end-effector are given by 
the coupled equations 



p r + G r (x r ) = w r + w„ 
p + G(x) + (C x )A = w d -w, 
subject to C(g,x) = 



(4.7) 
(4.8) 
(4.9) 



where G r is the gravity wrench on the robot, G is the gravity wrench on the end- 
effector, w r and w<2 are the applied robot wrench and a contact disturbance wrench, 
C x is the derivative of the constraint equation C(g,x) = with respect to x, and A 
is a Lagrange multiplier. The product (C X )A is a constraint vector in the cotangent 
space of the contact constraint. The disturbance wrench lies entirely in the null space 
of the constraint wrench. To simplify the notation let C x = C x . The constraint 
equation can also be expressed as the inner product of the generalized velocity and 
the cotangent vector space. 

C^x = 0. (4.10) 
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Adding the momentum equations gives the total momentum equation 

p t + G r + G + C x A = w r + w d . (4.11) 

Now assume that the robot controller takes the form of a generalized spring and 
damper with gravity compensation and applies its wrench with a zero mean random 
error 

w r = K r (x d - x r ) + B r (v d - x r ) + G r + G + w. (4.12) 

In order to implement this equation, we assume that x r — x is small, so that G can 
be implemented as G(x r ) creating only a small error. 

Changing the control equation into momentum variables and substituting the result 
into equation 4.11 yields the approximation for the total momentum 

p t + C X A + B r x t + K r x t = w + w d (4.13) 

p-H t x = H t v d (4.14) 

where x t = x r — x^ is the tracking error and x t is the derivative of the tracking error. 

Now, we rewrite the dynamics of the end-effector in error coordinates. Let x = x — x r 
be the difference between the end-effector configuration and the robot's configura- 
tion and let the momentum of the difference be p = Hx. Then equation 4.8, the 
momentum of the end-effector, can be expressed as 

P + Bi + Ki = -G(x) - C X A - -[Hi] + w d (4.15) 

at 

since the wrench measurement is w m = Kx + Bx. The statistics of the resulting 
pair of equations can now be treated by hrst solving for the statistics of the total 
momentum, including the effects of constraint, and then using the result as part of 
the forcing function in equation 4.15. 

Before considering the statistics for constrained motion, it is illustrative to examine 
the statistics for free flight with a constant desired velocity. In free flight, the expected 
value of a hrst order expansion of 4.13 and 4.14 is 

— E[p t ] + B r — E[x t ] + K r E[x t ] = (4.16) 

at at 

E[p t ] - H t -^E[x t ] = H t v d . (4.17) 

at 

For a stationary solution, the expected values must be constant yielding 

E[p t ] = H t v d E[x t ] = (4.18) 
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The covariance of the coupled system is best represented by rewriting the equation 
for free flight as 



Id B r 
H t 



c + 



c 



K 
-Id 

M + B t c 




H t 

u + S t w 



v d + 



Id 





w 



(4.19) 

(4.20) 



where the matrices are defined appropriately and £ = [p^ x^]. With this notation, 
the steady-state covariance of £ , V/- is the solution of the continuous time Lyapunov 
equation and satisfies 



B t V c .4 t T + A t \ c Bj = S t V w S 



where V w is the covariance of the control error. 



(4.21) 



Similarly, the difference momentum equation for free flight is given by 



Id 
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Bi/ = 


"-G(x 




Id 



d 



J dt 

S^[Hx r 
dt 



Hx r 



(4.22) 
(4.23) 



where v T = [p x]. Taking expectations of a first order expansion gives the sta- 
tionary solution 

KE[x] = -G(x) - Hv d E[p] = (4.24) 

since ^rE[Hv r ] = ^r(H^) and v^ is constant. This shows that the expected mea- 
surement is affected by both the gravity wrench and the changes in inertia for the 
moving frame. The driving term in 4.23 is ^-[Hx r ]. Its covariance is 



V[(A [ Hx r] )(A [ Hx r] )- ] 



iHH" 11 
dt v f ' 



FV^F T 



iHH" 11 
dt v f ' 



> H - 



FV C B^" T F T [(HH- 1 ; 



^h-^Jf^b^f 3 



d 
dT 



HH, 1 ) 



+ [(HH7 1 )] F^T 1 [B t V c Bf + S t V w S t T ]^l- T [(HH^f 



(4.25) 

(4.26) 

(4.27) 
(4.28) 



where F = [Id ] selects the momentum component of £. The control disturbance 
creates measurement variance both directly and through its effect on the variance of 
the robot velocity. This effect will be treated as constant and incorporated into a 
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single modified variance V Wm . This is justified for slow changes in the inertia tensor. 
Using this definition for the driving variance, the variance of v satisfies 

BV I y^ T + ^V I yB T = SV Wt „S T . (4.29) 

Finally let D = [BH 1 K], then Di/ = w m and the variance of the measurement 

isDV I/ D T = V Wm . 

Now, consider the equations of constrained dynamics. For constrained motion, the 
average dynamics of the total robot looks like a point being pulled through a gen- 
eralized spring and damper. The motion complies to the constraints because the 
average constraint wrench exactly balances the effect of the component of the com- 
mand into the constraints. These wrenches deflect the connecting spring and add 
to the measured wrench. In addition, the expected value of the contact disturbance 
force also adds to the average measured wrench. Velocities which have a non-zero 
average will create an average bias disturbance through the effects of friction. 

Besides the control error, the variance of the process depends on the variance of 
the contact disturbance and errors in the constraint model. These disturbances are 
caused by stick-slip during motion and textures and other small features. This second 
set of disturbances are velocity dependent. The equations make this precise. 

The constraint dynamics equations consist of three coupled equations 

p t + C X A + B r x t + K r x t = w + w d (4.30) 

Pt -H t x = H t v d (4.31) 

C x T x = C x T v d . (4.32) 

The stationary solution requires C x v^ = 0. Then p t = H t Vd since the velocity 
is compatible with the constraints. Finally, errors in position are balanced by the 
average disturbance force and the constraints K r E[x] + C x E[A] = EfwJ. A more illu- 
minating assumption is that ^-E[pt] = 0, that is the system behaves quasi-statically 
on average. 

The constraint dynamics equation 4.14 can be solved for x t and the result substituted 
in to the constraint equation 4.10 to get 

(C x T B r " 1 C x )A = C x T B r " 1 (-p t - K r x t + B r v d + w + w d ). (4.33) 

We have assumed that B r is invertible. If we define the projection matrix 

P = B^Id - C^C/B^C^^C/B- 1 ) (4.34) 
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then the error dynamics can be rewritten as 

Pp t + PB r x + PK r x = P(w + w d ). (4.35) 

Note that the projection matrix has the property PC X = 0. Therefore, the projection 
matrix takes covectors and projects them into the tangent space of the constraints. 

Taking expectations and applying the assumption ^E[p t ] = 0, a hrst order differen- 
tial equation results for the robot velocity 

-£-E[x] = P(-KE[x] + B r v d + E[w d ]) (4.36) 

at 

Therefore, the average velocity is the result of projecting the control wrench B r v^ — 
KE[x] plus the average disturbance wrench onto the tangent space through the 
damping matrix. The average momentum is the product of this velocity and the 
inertia tensor. 

This also implies that the covariance of the momentum is not of full rank. This shows 
statistically that contact constraints reduce position uncertainty. The covariance of 
the momentum and tracking error is again a steady-state solution to a Lyapunov 
equation which can be derived using the projected form of the dynamics 4.35. More 
importantly, the constraint on velocity can be rewritten as a constraint on momentum 
yielding 

C x T H t - 1 V Pt H t - T C x = (4.37) 

as a constraint on the covariance of the momentum. Since H t is full rank, this 
condition implies that V Pt has zero components in the directions of the constraints. 
The covariance is driven by both the control disturbance and random errors in the 
contact wrench. 

The last derivation assumed that the constraint equations were known exactly. How- 
ever, small surface imperfections or textures produce a small variability in the con- 
straint equation. Therefore, to hrst order the constraint equation can be expanded 
to 

(E[C X T ] + «5C x T )H t - 1 (E[ Pt ] + 6 Pt ) = 0. (4.38) 

By assumption E[C X ]E[p t ] = 0, therefore, 

5C x T H t - 1 E[p t ] = -E[C x T ]H t -^p t (4.39) 
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to first order. Therefore, the constraint on the covariance is modified to 

E[C x T ]H t - 1 V Pt H t - T E[C x ] = E[«5C x T H- 1 E[ Pt ]E[ Pt ] T H- T «5C x ]. 



(4.40) 



Instead of having zero component in the direction of the constraints, the covariance 
now has a component in the direction of the average constraint which depends upon 
the square of the projection of the momentum into the error in the constraints. In 
actual practice, this velocity dependent error will produce measurement variance 
components in all directions. The magnitude of the variance will depend upon both 
the direction of travel and the square of the travel speed. Direction is important 
because many materials may have directional structure to their textures. 

Now consider the difference momentum equation 4.15. The constraint modifies this 
equation to 



-G(x) 




S — [Hi 

dt L 



C x 





Ai> + Bi/ = 

Therefore, taking expectations, gives the stationary solution 





E[p] 
KE[x] 
E[w m ] 



-G + Hv d -E[C X A] 
G + E[C X A] - Hv d . 



(4.4i; 



(4.42) 
(4.43) 
(4.44) 



The covariance of the measured force takes the same form as in free flight, but with 
the addition of a noise component that depends on the errors in the constraint. 

In summary, the expected motion of the robot at steady-state on a single contact 
manifold can be described by mapping the applied wrenches onto the tangent space. 
Motion then occurs along the tangent space. The constraint wrenches necessary 
to achieve this motion cause a control error and create a possible large average 
measured wrench. The form of constraint wrench depends upon the type of the 
contact for systems with a finite number of types. Therefore, in order to predict 
the expected value of the measured wrench, the constraint type and the parameters 
of the constraint must be estimated. This is the topic of chapter 8. Second, the 
variance of the constraint wrench depends upon the contact disturbances, the robot 
control error, and on a velocity dependent term from the variability of the constraints 
through a Lyapunov equation. This velocity dependent term can depend both on 
the direction of the velocity and on the magnitude. Chapter 7 considers this velocity 
dependent term by examining the temporal characteristics of the noise from the force 
sensor. The dynamic effect is changes in constraint which is considered in the next 
section. 
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The equations can also be used for systems that are accelerating on a single mani- 
fold. In this case, the change in momentum will be measured by the sensor and the 
measurement covariance must be treated as time varying. 



4.3.2 Changes in Constraint 



There are only two types of changes in constraint: 1) changes from a manifold of 
lesser constraint (higher dimension) to one of more constraint (lower dimension), or 
2) changes from lower dimension to higher dimensional surfaces. It is impossible to 
cross between two manifolds of the same dimension without hrst passing through 
either the larger surface they are embedded in or through their intersection. 

An impact wrench will result from the hrst kind of change if the velocity causing the 
transition is not tangent to the new more constrained surface. Impacts result from 
transition velocities which violate the constraints of the new surface. For example, 
to avoid an impact wrench a point in 3J 3 which is going to contact a plane must have 
a crossing velocity which lies entirely in the plane. This sort of motion can be seen 
as a limiting condition on a smooth transition motion. This can occur when two 
constraint manifolds smoothly blend into each other. 

The second type of transition may or may not result in an apparent impact wrench. 
The same ideal point will not experience an impact when it leaves the plane, because 
the higher dimensional space can accommodate the entrance velocity. However, 
any real robot has some built up potential spring energy in the sensor structure, 
the contacting linkage, and the robot joints. During contact, this energy is being 
contained by the constraint surface. If contact is lost abruptly, a contact loss impact 
occurs because of the sudden release of this energy. Abruptness is a function of the 
bandwidth and damping of the sensor and the speed of the transition. 

For the hrst type of transition we need to distinguish between stable and unstable 
transitions. An example of an unstable transition is an object being pressed into a 
surface and then dragged along that surface. When the orientation of the surface 
discontinuously changes (i.e. we cross an edge), a sudden change in the orientation 
causes the object to lose contact with the surface and a loss impact results. 

A transition is stable if the transition into the intersection between two manifolds 
remains in the intersection instead of continuing onto the next manifold. To formalize 
this let Aii and A4j be two constraints in the configuration space with non-empty 
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intersection Aiij = Aii f] Aij. Let their co-tangent bundles be defined so that the 
set of wrenches they can support is given by the positive convex combination over 
the basis vectors in the co-tangent space and the disturbance vectors. The set of 
wrenches supported on Aii at x is then Sup 8 (x) = Convex(T* (x) } Range[wJ(:c)). 
The range value models the friction component which also depends upon the applied 
wrench. 

A state x and control v^ will be totally stable, resulting in no motion, if the applied 
control can be entirely supported by the constraint 

K r x-B r v d eSu Pt -(x). (4.45) 

A less strict form of stability is that the control plus the resulting constraints result 
in a motion in the tangent space of the intersection manifold Aiij. This definition 
is more useful because it states that motions on a manifold stay on a manifold. This 
will be the case if at state x and control (x^, Vd) there is a nonnegative solution for 
A in 

(C x T B r - 1 C x )A = C/B^l-Kri + B r v d + E[w d ]). (4.46) 

Such a triple (x, x^, vA will be called manifold stable. 

In order for a transition from Aii to Aij to remain on the intersection, several 
conditions must hold. First, the control v^ and the transition state x must be 
manifold stable on Aiij. Second, the control and state must be only manifold stable 
on Ai{. If this condition does not hold, the robot will become stuck on Aii and will 
never transition. Lastly, the velocity that results from the control on Aii must have 
a component that causes motion toward the intersection. 

In an ideal stable impact the momentum changes discontinuously to bring the new 
velocity into the new tangent space. Let the initial manifold be Aii and the new 
manifold be Aij. Let v new be the velocity after transition and the velocity before 
transition be v trans . v new must lie in the tangent space of Aii }J so that C 8J (x) T v neu , = 
0. The change in momentum is caused by an impulse which lies entirely in the 
cotangent space of Aij. The equation for the change in momentum is 

H t (x)v netu = H t (x)v tr(ms + C,(x)k. (4.47) 

Using this relationship, the constraint gives 

k = -(CjHfC^-'CjH^p^ (4.48) 

^impulse = Cjk (4.49) 

Pnew = (Id-C J (CjH t C J )- 1 CjH t - 1 )p oM (4.50) 
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Figure 4.9: Impact transition from manifold Aii to manifold Aii, 



3' 



for the impulse force and new momentum. The impulse force is generated over a 
small window of time 

i-t+St 
Vf impulse = / E[w(t)]dt. (4.51) 



The impact wrench adds into the measured wrench. The idealized model for E[w] 
is a delta function at time t. However, in actuality the workpiece bounces several 
times, in a complex way, before settling onto the new constraint. Energy is lost on 
every bounce due to damping in the robot and at the interface. The integral can be 
computed from the data if the beginning and end of the impact can be identified. 
Chapter 7 discusses this identification problem. The change in momentum can be 
used to help determine the new constraint manifold. 

For an unstable impact, there is a release in potential energy. The change in the 
average force stored in the spring is 



K(E[x 



after 



E[x 



before , 



E[C X A, 



before J 



E[C X A ( 



after \ 



(4.52) 



This difference in wrench adds an impulse wrench into the measured wrench. The 
impulse causes a transient response which slowly dies out. The direction of change 
can be found by identifying the peak wrench. 
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4.4 Conclusion 



This chapter formulated the constraints and dynamics of rigid bodies in the config- 
uration space of allowed motions. The configuration space was shown to have an 
intrinsic topology graph. Each node in the graph represented a region of constant 
dynamics. The dynamics were shown to depend critically on the type and geomet- 
ric description of the constraint. It was shown that different geometric descriptions 
could result in the same constraint. 

For constant velocity motions on a single configuration space manifold, the stationary 
solution to the expected value of the motion was shown to take the form of a point 
sliding on the manifold pulled by a spring and damper. The statistics of the motion 
and the measured wrench were shown to depend upon not only the control and 
contact disturbances, but also on the direction and magnitude of the motion. 

Transitions between nodes in the graph were shown to correspond to transitory 
events. Transitions to manifolds with more constraint were shown to be either stable 
or unstable. Stable transitions almost always result in impacts. Unstable transitions 
result in loss impacts if there is stored elastic energy. 
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A Contact Feature Model 



Chapter 5 



The core contribution of this thesis is an approach to building an observer for manip- 
ulation. The goal of any manipulation observer is to estimate the pose and relative 
motion of salient objects in a timely fashion. Observers can be built using a complete 
C-space representation [Robles, 1995], or local threshold tests on the signals can be 
designed to trigger events and, in effect, observe the relevant information. 

A C-space approach is powerful. It represents all of the relevant information and pro- 
vides a convenient geometric framework for information fusion. On the other hand, 
current C-space representations become computationally prohibitive and memory 
intensive for high dimensional systems. The potentially high computational cost 
may make it difficult to produce timely results. Local event triggers have the op- 
posite properties. They are quick to make decisions, but do not provide a global 
measurement fusion framework. 

This chapter discusses a model based approach that blends the two representations. 
We use models of the measurement process to produce contact feature signal descrip- 
tions. Each of these models provides a statistical description of the measurement 
process applicable to a small patch of phase-space. A fast, robust, local test can 
then be derived from the model using statistical decision theory. 

Global knowledge is incorporated by generating a graph of possible contact features. 
This graph can be either programmed for local tasks, computed from geometry and 
hrst principles, or learned for repetitive tasks. Essentially, a local test determines 
that there has been a change in the contact conditions and the graph determines the 
possible new models. 
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This approach can produce timely results and incorporate global context. In addi- 
tion, since change tests are derived from models, the assumptions made in picking 
a test threshold are explicit. Regions of C-space are lumped together in the model. 
This has the advantage that during a motion signals arising from a particular model 
will tend to persist. This makes it easier to detect the model and track the mo- 
tion of the robot. However, geometric information about length and shape is not 
represented and is not available to the observer. 

A later chapter looks at features that are relevant to the mating and assembly phase of 
rigid body manipulation. The ideas can be extended to other phases of manipulation, 
but this was not experimentally explored during the course of this work. This chapter 
will discuss some possible ways of applying the ideas to other areas of manipulation 
such as grasp control. 

This chapter defines a contact feature and discusses what is and what is not a feature. 
We then discuss how contact features can be connected to form a graph called feature 
graph. The following chapter then presents an observer designed to work with the 
feature graph to estimate the current state of the manipulation task. The remaining 
chapters in this thesis develop the estimation algorithms for the basic features that 
arise in environments composed of rigid bodies. Two basic features are developed: 
1) the constraints on motion caused by contacts, and 2) the temporal effects caused 
by changes in constraint or velocity dependent temporal characteristics of contact 
forces. 



5.1 Definition of a Contact Feature 



A useful definition of contact feature is needed to make our model based approach 
precise. A contact feature is 



• a statistical parameterized measurement model, 

• which is applicable at every point in phase-space, 

• and which partitions phase-space into a countable number of sets. 



A statistical representation makes it possible to apply the powerful machinery of 
statistical decision and estimation theory to determining the current model. The 
second requirement makes it possible to compare signal measurement histories. 
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The last requirement is the most important. This requirement ensures that a discrete 
graph will actually result from a given model and rules out some common models. 
This requirement is a function of both the chosen measurement model and the under- 
lying phase-space. Models that are features in one phase-space may not be features 
in another phase-space. 

We need to be able to take common measurement contact models and make them 
parameterized , statistical models. The parameters of any model will be indicated 
by 6 . Most common measurement models can be placed in this form. 

For example, the measurement of the position of the robot y might be described by 

y(t) = x(t) + U(t) (5.1) 

where v(t) is an independent, identically distributed (i.i.d. ) normal process with zero 
mean and variance V: 

u(t) -i.i.d. N(0,V). (5.2) 

The parameters of the distribution for y(t) are x(t) and V. 1 

Friction for a point robot on a plane can be put in this form. Relative motion of the 
robot breaks the model into two possible cases. For the fixed position case, a model 
for the measured force w m (t) = (f x (t) } f y (t)) is that it has a independent, identically 
distributed, maximum entropy distribution over the friction cone specified by a fixed 
normal n and coefficient of friction //. For the moving case, a model for the measured 
force is the distribution 

/*(*)-///,(*) sgn(i;) = u(t) (5.3) 

v{t) ~ i.i.d. N(0,V). (5.4) 

Therefore, in the friction model case the hrst parameter of the distribution for w m 
is an index giving the appropriate case. The rest of the parameters hx the required 
probability distribution. 

Parameterized statistical models can also be formed from constraint equations be- 
tween the measured/grasped part and other objects in the environment. This is a 
very important class of model. Constraint equations can always be written in the 



1 It will often be the case that only conditions on probability distribution are available. In that 
case, we will adopt the maximum entropy distribution as the underlying distribution [Cover and 
Thomas, 1991]. The maximum entropy distribution for a real random variable when the mean and 
variance are fixed is the normal distribution. The maximum entropy distribution for a real bounded 
random variable is the uniform distribution. 
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form C(x,g) = which requires that the robot's phase x satisfy the constraint pa- 
rameterized by the geometric parameters g. In general, constraints hold only for a 
range of configurations. Free space is the null constraint equation. 

The constraint equation can be turned into a measurement equation by projecting 
the measured force onto the tangent space and projecting the measured velocity on 
to the cotangent space (at a configuration). In a perfect system without any noise, 
modeling error, or friction these projections would yield zero. In the real world, the 
projection will be nonzero and the result can be modeled as an independent noise 
process: 

C*(x) T w m = i/ w (5.5) 

C x (x) T x = i/ x (5.6) 

where C* (x) is a basis for the constraint cotangent space, C x (x) is a basis for the 
constraint tangent space, w m is the measured force, x is the measured velocity, 
and i/ w and v± are independent measurement processes. We use zero mean normal 
distributions for the measurement error statistics. The statistics of the measurement 
processes may or may not depend on the velocity. In either case, if we have a 
model for how the statistics depend on the velocity, a model which is applicable 
everywhere can be generated by setting the hrst parameter to be the constraint 
model type and letting the remaining parameters specify the geometric parameters 
and the measurement statistics. 

We now formalize the definition of contact feature. The measurements y(t) (the 
force, position, both, or something else entirely) are assumed to come from a model 
of the form 

y(*)~i.i.d.h(x(*),x(*),0) (5.7) 

All of the previous models are of this type. Now for a pair (x, x) in the phase-space, 
let M(Q) be an instance of model M which applies to this pair. The instance is 
specified by the value of 6. Models are defined so that only one instance applies to 
any one pair. The set of all points where a given model applies is the domain of 
definition of the model when specialized by fixing the parameter value. 

The set of all possible domains, for all possible values of 0, forms a partition of the 
phase-space. A stationary feature will be any model which produces a countable par- 
tition of the phase-space. The domain associated with each instance of the parameter 
is defined as the feature instance partition, and the feature model specialized by 6 is 
a feature instance. 



5.1: Definition of a Contact Feature 79 



The fixed normal friction model for the measured force is a feature for a point robot 
in a countable polyhedral world. The hrst parameter, the model type, partitions the 
phase-space into (C,x = 0) and (C,x ^ 0). If x = 0, the rest of the parameters 
are the vectors describing the convex cone. If x ^ 0, the rest of the parameters 
are the parameters of the cone and the statistics of the model error. Any one cone 
applies to an entire face, edge, or vertex of the polyhedral obstacles. Since there 
are a countable number of such geometric features there are a countable number of 
partitions for x = 0. If the number of model error variances is also countable, or 
they can just be lumped into a single statistical model, then there are a countable 
number of partitions for x ^ 0. Therefore, cones are a feature for this phase-space. 

However, the fixed angle friction cone model is not a feature for a polyhedral object 
in a polyhedral world. This is because the direction of the friction cone depends 
upon the orientation of the moving object. Therefore the domains depend upon the 
orientation, which yields an uncountable partition. This emphasizes that the feature 
definition depends both on the statistical model and the underlying phase-space and 
implicitly the configuration space. 

Contact constraints models are features for polyhedral and curved environments. 
This is clear because there are only a few contact constraint types for contacts 
between polyhedral objects. Each of these types corresponds to one of the partitions. 
In fact it is clear that constraints are a feature for any rigid world consisting of 
piecewise smooth surfaces. Since this is a very broad array of objects, constraints 
are obviously a very important class of feature. The contact constraint models can be 
extended to incorporate friction and this in essence makes the friction cone normal 
depend upon the configuration. 

Our definition of a feature can be contrasted with an alternative concept of disam- 
biguous sets [Buckley, 1987]. We will discuss the complementary sets the confuseable 
sets and restrict our attention to partitions of the configuration space. The argument 
also applies to phase-space. Confuseable sets are defined by defining two points in 
configuration space as equivalent if they are both consistent with a given measure- 
ment. That is both points are in the same interpretation set for a given measurement. 
The extension is that two points are equivalent if it is possible to generate a mea- 
surement which will place them in the same interpretation set. Essentially, a pair of 
points in the configuration space (xi,x 2 ) is confuseable if it is possible to generate 
a measurement for which we cannot decide with certainty if the measurement came 
from Xi or x 2 . It remains to test if this is an equivalence relation. 
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Lets see how the confuseable set definitions works for fixed angle friction cones in a 
polygonal world. Clearly all the points on one edge are confuseable, because they all 
have the same friction cone. The points on two edges which form a vertex will not 
be confuseable if their relative angles is greater than the friction cone angle. 

Now suppose the world has a circle that the point can contact. Select three points 
Xi,x 2 ,x 3 on the circle so that the friction cones of Xi and x 2 share some directions, 
and the friction cones of x 2 and x 3 also share some directions, but Xi and x 3 do 
not. By definition Xi and x 2 are confuseable and so are x 2 and x 3 . Now in order 
for confuseability to be equivalence relation Xi must be confuseable with x 3 , but by 
construction it is not. Therefore confuseability is not an equivalence relation because 
the relation is not transitive, and therefore it cannot be used for constructing a graph. 
Clearly this problem also arises for polygonal objects in two dimensions. The model 
equivalence definition survives the test because every point is only equivalent to 
itself. Of course, this implies that fixed normal model of friction is not a feature in 
configuration spaces with curved surfaces. 

The fact that sensor noise can destroy transitivity in the definition of confuseable sets 
was recognized in [Donald and Jennings, 1991]. Their work defined a configuration 
space perceptual equivalence sets in a manner similar to disambiguous sets. 

The essential difficulty is that ambiguity or disambiguity is too strong a constraint to 
place on the problem. Disambiguity requires two sets to be uniquely separable with 
one measurement. Probabilistic measurement models provide only the possibility of 
drawing increasingly more likely conclusions with additional data but certainty is 
never guaranteed. 

Features can be combined. Many possible features could exist for a given set of 
measurements and task. For example, a tactile array sensor could be used to compute 
the contact curvature, the contact stiffness, and possibly relative motion all of which 
are features. Each feature provides a different partition of the phase-space. The 
intersections of these partitions is again a countable partition, so a collection of 
features is again a feature. Combining features yields a finer partion of phase-space 
and in general this helps in both identifying the current state of the robot and in 
selecting the appropriate control action. 

Nonstationary models are much more difficult to handle. Impacts will be the only 
nonstationary model needed for this thesis, because the only nonstationary events 
that will arise in this work are transitions between contact constraints. We will 
consider this type of model to be a transition feature because impacts occur only 
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when transitioning from one stationary feature to another stationary feature. 



5.2 Properties of Useful Features 



The last section provided a definition of a feature. A constructive method of de- 
termining available features for a phase-space and dynamics is not yet available. 
However, we can comment generally on what makes a good feature. 

The fundamental assumption behind the feature approach to programming is that 
a single continuous control law will exist for each feature instance. Features should 
have this property. The exact parameters of the control law may then depend on 
the other non-feature properties of the configuration space. However, the parameters 
are assumed to vary smoothly within the partition for a single feature instance. For 
example, the form of the hybrid force control law is determined by the form of the 
constraints which is a feature. The direction of constraint and free motion is then 
determined by the current contact location. 

Although we can test this property given a control law and feature definition, we 
cannot use this property as a guide to feature selection because of the interplay 
between control strategy and the feature choice. Considering both together would 
place us back in the pre-image framework. A simpler test is to choose features that 
are adapted to the dynamics. As discussed in the last chapter, the dynamic equations 
of the complete system generally take on different discrete forms depending upon the 
contact conditions. For a single rigid body, the dynamics change whenever a contact 
constraint is made or broken. The different forms for the dynamic equations also 
create a partition of the phase-space. A good feature would produce a partition 
which is adapted to the partition created by the dynamics. That is, given a feature 
instance, the form of the dynamic equations should be uniquely determined. 

The assumption behind this requirement is that a complete control algorithm can 
be written for accomplishing the task by analyzing the dynamics for the different 
possible dynamic equations. We assume that a local controller can be written for 
each dynamic partition which will locally keep the robot on a desired path. This 
approach was shown to work for pushing by [Narasimhan, 1994]. 

Finally, it should be relatively easy to determine, at least pointwise, feature member- 
ship for points in phase-space. It would be even better if it was possible to compute 
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the feature domains. 



5.3 Encoding Tasks with Features 



In order to accomplish a task, the partitions induced by the features have to be 
organized for the given task. There are many possible organizations. We will dis- 
cuss two possibilities: feature connectivity based on topologically connectivity, and 
feature connectivity based on forward projection. Both definitions yield a feature 
graph. The graphs are based on the underlying connectivity of configuration space. 
A graph is produced which can be used to infer information about the state of the 
manipulation task. Which organization is most useful for a given task depends upon 
the complexity of the problem at hand. 



5.3.1 Connectivity from Topological Path Connectivity 



As discussed in the last section, a collection of stationary features creates by definition 
a partition of the configuration space. A graph of this partition can be constructed 
by using the tangent space connectivity of the underlying configuration space. We 
identify a graph node with each element of the partition. Let the set of all nodes be 
J\f. Every node Afj can be associated with a label, and a vector of parameters for 
that node, and its associated partition 

M 3 = (j,e k3 ,V 3 ) (5.8) 

where j is the label, kj is an index into the set of possible model instances, and Vj is 
the partition associated with the model instance. For any pair of nodes in J\f create 
an edge connecting the nodes if there exists a configuration in the hrst node and a 
configuration in the second node which are connected in the underlying configuration 
space. Let 7r x be the natural projection from phase-space to configuration space, and 
let 7Tx be the natural projection from phase-space to the tangent bundle. Then we 
have the following definition. 



Definition 5.1 The edge £(i,j) exists if there exist x G ffxCPi), x i G ^("P?), and 
an integral curve ait) such that: 

1. cr(0) = x , and <r(I) = Xi, 
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Figure 5.1: A feature graph based on path connectivity. The moving 
block is constrained to move rectilinearly in x and y. The force sensor 
measures the x and y contact force, so the contact feature model is 
determined by the contact normal. Each configuration space surface 
which shares the same contact model has been indicated with the same 
letter. This induces the indicated configuration space with four different 
contact constraint models. The partition for each model consists of 
disjoint domains in configuration space. The graph is induced by the 
connectivity of the domains. 



2. o-(t)C7r x (Vi)[j7r x (V 



i )i 



3. and ait) = Y^\ a iif)Bi{cr{t)), where {i? 8 (x)} is a basis for the tangent space 



Let the collection of all such edges be £. The resulting graph Q = (A/", £) is called 
the feature graph. This notion of feature connectivity produces a very coarse graph, 
because it lumps unconnected components of the phase-space together. However, it 
can be used to accomplish useful tasks, if we limit the size of the graph by using 



spatial locality. 



Spatial locality limits the range of the phase-space under consideration to a partic- 
ular volume. This volume is chosen because the system knows the robot is in the 
particular region from other measurements or prior knowledge. 
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Figure 5.2: An idealized grasp maintenance problem. The rollers are 
only to indicate constrained sliding motion. We assume that the robot 
is touching the block for this tasks so that x r = w. With this simplifi- 
cation, the configuration of the parts are specified by the height of the 
robot y r and the height of the block y b measured with respect to the 
global coordinates. The configuration space is therefore the set of pairs 
(y r} y b ). The robot task is to lift and replace the block. 



A finer local partition can be constructed by splitting each node in the graph into 
components which are topologically path connected in the phase-space. In general 
computing if two sets are topologically connected is difficult. 

The nodes created by this splitting operation will no longer be uniquely labeled by 
the feature instance for the points. Instead any feature instance will correspond to a 
collection of nodes. In figure 5.1 the single node representing feature a would become 
three nodes. However, if we can assume that the robot remains in a local subset of 
the graph, we can prune the extra nodes. The resulting local feature graph can be 
used for controlling the robot in a small region of phase-space. 

This notion of connectivity, along with locality, can be used as a basis for finite state 
based control. A grasp maintenance problem shown in figure 5.2. From locality the 
robot is known to be in contact with the block. Only lifting and slip need to be 
controlled. Therefore, the configuration space for this problem is the pair (y r} y b ). 
The stable contact configurations which can occur during grasping are 



Cx = {y r ey b + [-h/2,h/2]&ndy b = h/2} 
C 2 = {y r ey b + [-h/2,h/2}cindy b >h/2} 
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These two pieces of configuration space are clearly connected to each other. 

The features of slip and contact force based on friction can be used to control the 
grasp. We assume the robot can sense vibration and force. The construction starts 
by defining the features for the vibration v and force w m measurements, and then 
each feature is associated with its partition of the phase-space. The measured force 
is the reaction of the environment to the force applied by the robot. We consider 
only partitions of C\ and C 2 

Slip is indicated by the magnitude of the vibration. There will be a higher vibration 
level when the block is slipping then when it is not slipping. Vibration can be 
sensed in a number of ways. One way is to look at the magnitude of the energy 
in the derivative of the force signal. Another is to measure the magnitude of an 
acceleration signal, or the magnitude of stress rate sensor such as a piezoelectric 
him. 

The measured vibration when there is no relative motion can then be modeled as a 
normally distributed variable given by a calibrated mean and variance. So under the 
no-slip condition, the vibration measurement has the model 

u(*)~i.i.d.N(// ,Ki), 
where N is the normal probability distribution. 

The statistics for when the sensor is slipping can either be calibrated, or an exponen- 
tial model (given a variance) could be used for the difference between the measured 
signal and the no slip vibration mean. This model for the vibration signal under the 
slip condition is 

(v(t) - ji )/V v2 ~ i.i.d.Exp 

where Exp is the unit exponential distribution 

Exp(x) = e~ x 



The measured contact force has three possible states: contact, lifted, and falling. 
When the robot is in contact, but has not yet lifted the block, the force must lie in 
the friction cone. The distribution of the force can be taken as the maximum entropy 
distribution over the cone. When the block is lifted, the y force must have a mean 
equal to the weight, and the x force must be sufficiently large to support the y force 
through friction. Finally, if the block is falling, the x and y forces must be smaller 
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than the required lifting forces. The weight of the block is fw a positive number, 
and w m is the measured force. 

Force : 

Fi w m (t) ~ i.i.d. ME(|/j,| < fif x ,f y > -fw,fx > 0) => Contact 

F 2 f y ~ i.i.d. N(-./V, y /2 ), /* ~ i.i.d. ME(/ E > / w ///) =* Lifted 

^3 w m (t) ~ i.i.d. MEd/J < nf x Jy > -f w ,0 < f x < fw/fJt) =>• Falling 

where ME is the maximum entropy distribution over the indicated set. 

Each sensor partitions the phase-space into components. The partitions for each 
feature instance are: 
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where <f> indicates the empty set. 

Now apply definition 5.1 to connect the partitions. Since C\ is on the boundary of 

C 2 and there exist velocities on both manifolds that will cause transition between 

the manifolds all the the features are connected to each other. The definitions only 

requires that points in configuration space be connected not that the velocities match 

in the two partitions. Therefore, the feature graph is 

P „ p 

' F V V S2.F1 

t X t 

P P 

S1,F2 <* ► S2F3 
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Since each feature in this graph is unique, observing the current feature instances is 
trivially equivalent to determine the state of the robot in the feature graph. This will 
be sufficient if the feature instance is enough to determine the appropriate control 
algorithm. This will be the case for essentially differentially local problems, such as 
this grasping example. In other words, if the control problem can be broken down 
into: I) finding a finite set of local controllers, 2) selecting the appropriate controller 
based on a unique feature instance, and 3) the controllers will keep the robot within 
its local domain of definition, then this is a good representation. When the control 
law also depends upon the location of the feature instance in the configuration space, 
this representation is not sufficient. 

The graph can be extended to incorporate transient features. Every transient feature 
corresponds to an edge in the initial task feature graph. Replace every such edge 
with an additional node labeled with the transient feature. If the transient feature is 
guaranteed to occur on transition from the initial to final feature node, connect the 
initial node to the transient node, and then the transient node to the final node. If 
it is not guaranteed to occur also connect an edge from the initial node to the final 
node. 

As a very simple example, of transient features, consider the feature of the existence 
of contact. A simple contact model is 

w m (t) ~ i.i.d. N(0,Vi) for forces in free space (5-9) 

w m (t) ~ i.i.d. U(Measurable Forces) for forces in contact (5.10) 

where U is the uniform probability distribution over the given set. 

The two equivalence classes for this model are the free space T and its associated 
tangent space 7jr, and the configuration space obstacle surface O and its associated 
tangent space To- Therefore, the two equivalence classes in the partition of phase- 
space for this model are 

{(:f,t»,(0,t o )}. 



Labeling the hrst element I and the second as 2, gives the feature graph 

1 ) (2 




If impact transient features also sometimes occur when going from free space to 
contact, the feature graph becomes 
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Figure 5.3: Example path and feature graph for a point robot and 
a two dimensional hole. Although the path is the correct path, an 
observer based on the feature graph shown at the right will be unable 
to recognize termination because the transition from 8 back into free 
space will reset the observation process. The second time the upward 
normal is observered, the observer will report it as possibly coming 
from 1, 5, or 9. 




where 3 is the transient feature node. In this model, the impact node can only be 
visited when transitioning from free space to contact. In some cases, there may be 
sufficient stored energy in the sensor so that a release impact occurs when transi- 
tioning from contact back to free space. 



5.3.2 Adding Actions to the Graph 



The local approach of linking feature models based on phase-space connectivity can 
fail in a larger context. The approach can fail because the models become too 
connected, and therefore the graph provides insufficient constraint. This is illustrated 
in figure 5.3. 
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The starting configuration of the robot is above and to the right of the hole (feature 
F). A downward command is applied and the robot contacts the right of the hole 
(feature 9). The local feature graph indicates that three features exist which have 
the same normal: the side to the left of the hole (feature 9), the side to the right of 
the hole (feature 1), and the bottom of the hole (feature 5). The observer will return 
all three features and report them as equally probable. 

Now the robot is commanded to move to the left. This causes the robot to cross the 
edge and transition back into free space (feature 8 followed by F). The transition 
returns the observer to its starting condition of free space (feature F), the additional 
information that was gathered by moving and losing contact with the edge is not 
represented in the local feature graph. Therefore, a decision algorithm cannot tell 
from this representation that the robot is actually closer to the hole and that the 
next time the vertical normal is observered it must be from feature 5. A richer 
representation is required for this type of task. 

The example also suggests a solution. Predictions about the consequences of a given 
motion should be incorporated into the observer. Fortunately this can be done 
by making the feature graph dependent on the action. This is done formally by 
incorporating forward and back projection. It should be noted that computation 
of time projection of sets is a very hard task in general, and that the additional 
information being provided here by the action dependence is substantial [Canny, 
1987]. 

In addition, the geometric parameters estimated for the constraint feature contain 
information about the pose of the contacted objects. The estimates of these param- 
eters could be used to update an estimate of object configuration. 

Forward projection is prediction or simulation of the possible outcomes of an action 
given the current estimate of the range of configurations for the robot and its envi- 
ronment. The dynamics, the current feedback local controller, and the configurations 
of all the objects define the equations of motion for the robot. These dynamics will 
hold until termination is signaled from a decision algorithm, which uses the prob- 
ability estimates from the local feature observer. As long as a new trajectory and 
controller are not introduced, the motion of the robot can be represented with a 
single stochastic differential equation: 

dx = n 8 (x(t),u) + dv. (5.11) 

where n 8 - is the dynamics of the robot when in contact with points in feature node z, 
u is the control, and dv is the stochastic control disturbance. 
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Forward projections are defined using equation 5.11 following [Erdmann, 1986]. The 
forward projection of a set S is the set of all points that could be reached by the 
dynamics given the control error. Formally 

Definition 5.2 The unbounded time forward projection of a set S is 

J-"(S) = {x' : 3t > and <r dv (t)(0 such that <r dv (t)(0) G S and <r dv (t)(0 = x'} 

where <r dv (t) is a possible trajectory of the robot given a sequence of control distur- 
bances. 

Back projection is defined similarly. The unbounded time weak back projection of a 
set S is the set of all points that can reach S. 

Definition 5.3 The unbounded time weak back projection of a set S is 

B{S) = {x' : 3t > and <r dv ( t )(t) such that <r dv ( t )(0) = x'and <r dv ( t )(t) G S} 



Now back and forward projection can be used to create a graph. The results de- 
pend upon the controller and the action selection procedure. We will discuss some 
approaches that can be used for limiting the scope of the computation based on the 
expected properties of the observer. We also suggest a point based method which 
only approximately, and probabilistically, computes the feature forward projection. 



5.3.3 Equivalence under an Action 

Back projection and forward projection can be used to refine the collection of feature 
equivalent sets. Let V = {Pi} be a collection of feature partitions. For this section 
we will have to assume that the collection is finite. Finiteness is required because we 
will be considering a product set derived from the feature partitions. Each partition 
consists of points which have the same feature model and which are connected in the 
phase-space. To simplify notation here when referring to points in a partition Vi, we 
mean the points in the configuration space associated with the partition. 

Definition 5.4 Two points Xi and x 2 are equivalent under a single action if 
1. Xi and x 2 are elements of the same partition V{. 
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2. Every partition Vj reachable from Xi is also reachable from x 2 . 

3. Every partition Vj reachable from x 2 is also reachable from Xi. 



The second and third requirements simply ensure that the same feature transitions 
can occur from both Xi and x 2 . 

This definition can be related to weak back projections. For every partition Vi G V, 
let Bi = B(Vi). That is B{ is the set of all points which could reach partition V{. 
Now let X be an index set over all possible indices and let X be its complement in 
the indices. The index set partitions are defined as 

^=(n^)\(u»o, 

i.e. a point is in a power set partition if and only if it can only reach the feature 
partitions in the index set. Therefore, any two points selected from the same power 
set partition can both reach exactly the same partitions and are therefore equivalent 
under a single action. 

Figure 5.4 shows what this construction looks like for the classic cone model of 
uncertainty for generalized damper control [Lozano-Perez et al., 1984] . Generalized 
damper control only tracks the velocity command. The position is not used in the 
control law. Under this model of control, the path of the robot starting from any 
point will lie in a cone emanating from the point. Most of the power set partitions are 
empty. The hrst figure shows the original feature partitions and the cone of possible 
velocity commands. The figure to the right shows all the power set partitions that 
are not empty. The figure on the bottom shows how the power set partitions augment 
the original feature graph. The new nodes were connected using reachability. 

The index set used for each power set partition are given below. 
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Figure 5.4: A planar assembly problem showing the back projections 
from each feature and the intersection of the back-projections and par- 
titions. The control is generalized damper control with a cone model 
for the control error. 
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We have not connected partitions of free space that share only a vertex, because this 
transition has zero probability of occuring. 

This new graph solves the original motivating problem illustrated in figure 5.3. Now 
when the object leaves feature 8, after sliding along 9, it will enter T% instead of the 
entire free space. Therefore, the observer will know that the next contacted feature 
can only be 3, 4, 5 but not 9. 



5.3.4 Forward Projection Graphs and LCNP 



In LCNP (Local Control Nominal Path) [Narasimhan, 1994], the controller is as- 
sumed to keep the robot within an uncertainty ball about a nominal path using a 
stiffness type of control. The ball is at its maximum size in free space. Contact with 
a C-space surface projects the uncertainty ball onto the surface. The nature of the 
projection depends upon the model of friction. An uncertainty tube can be created 
by sweeping the maximum ball along the nominal path. This tube limits the range 
of feature possibilities and provides a control dependent feature connectivity. 

Figure 5.5 shows an uncertainty ball and tube around a nominal path for a point 
in the plane. To construct a graph, begin with the collection of feature equivalence 
sets. Now keep only those points in each set which also lie in the uncertainty tube. 
Now use a compliance model for action to compute the action equivalent sets and 
their connectivity to compute the desired graph. Figure 5.6 shows the result. 

A further approach is to remove the nominal path and consider the effect of an 
action everywhere in the space. The same definition can be used to produces a 
feature graph for every action. The collection of all such graphs will be a super- 
graph which is indexed by the selected action. We might use this super-graph to 
plan an appropriate action. 



5.3.5 Graphs and Nonparameteric Interpretation Set Rep- 
resentations 



For complex problems requiring a full knowledge representation, feature graphs might 
be computed on demand from the knowledge set and point-set simulation. Forward 
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Figure 5.5: Nominal path and uncertainty tube for a LCNP peg-in-hole 
strategy for a point robot in a two dimensional configuration space. 



Uncertainty Tube Divided 

into Action Equivalent Partitions 




Augmented Feature Graph 



F6- 



F7 




7.1 



7.2 



4 5 6 4« 5 M ° 

Figure 5.6: Action dependent graph for an LCNP insertion strategy. 
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projection of point sets in configuration space was addressed in the plane with rota- 
tion in [Caine, 1993] . These algorithms could be applied to computing the forward 
projection graph. We suggest, but have not implemented, the following Monte Carlo 
approach which does not require representing C-space explicitly. 

Represent the possible configurations of every object using a non-parametric distribu- 
tion. Each configuration is represented by a collection of n samples. Now randomly 
select a representative point from each of these distributions and do the forward sim- 
ulation. Record which features come into contact and which feature models become 
active. Every simulation produce one possible forward projection trace and pro- 
ceeds down one branch in the desired feature graph. Repeat the simulation until the 
features produced and the chance of hitting each feature becomes stably estimated. 

The simulation is likely to become stable quickly. The most likely constraint features 
will be found quickly in the simulation. The very unlikely features will take a long 
time to detect via simulation, but these features are irrelevant because they are also 
unlikely to occur in practice. We now have the desired feature graph which can be 
used by the observer. 

The same non-parametric distribution can also be used for incorporating informa- 
tion represented by the constraint parameter estimates. Every contact produces a 
constraint feature. The parameter estimates produced by the constraint estimator, 
after every new measurement, restrict the range of configuration for each object. 
The statistics of this restriction are also produced by the estimator in terms of the 
statistics of the relevant geometric parameters. 

For example, an estimator for a type B contact will return a normal n and a contact 
point p estimate and a covariance matrix for both. These two estimates produce an 
estimate of the vertex location for the contacting object, and require one edge of the 
contacted object to have normal n and pass thru p. This induces a distribution on 
the allowed configuration of each object considered independently. 

The information from the measurements can now be fused with the non-parametric 
distribution for each object independently. There are two distributions, the prior 
p(t — 1) and the new measurement distribution p m (t). These can be fused using 
random sampling. For each distribution generate n random points. There are now 
2n random points that represent possible locations of the object. Using this new non- 
parametric distribution, draw another n random samples. These n samples represent 
an estimate of p(t), the configuration distribution after measurement fusion. 
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The distributions for the objects are of course not at all independent. Many pairs of 
configurations for the two objects would produce a collision. This is explicitly repre- 
sented in configuration space. Furthermore, this information could be incorporated 
if the non-parametric distribution was defined in configuration space. This is the 
approach in [Robles, 1995]. However for n three dimensional objects the configura- 
tion space representation has complexity exponential in 6n. Because the approach 
outlined above ignores this information and fuses n independent distributions the 
complexity is only 6n. 



5.4 Conclusion 



In this chapter the idea of contact feature was formalized as a model-based description 
of the measurements. The hard requirement of disambiguity of sets was loosened to 
a definition in which sets can only be probabilistically separated. This was done by 
defining features as models which produce finite (or at most countable) partitions of 
the phase-space. This is critical, because a discrete graph for observing progress is 
only defined for features which generate a countable partition. 

Feature partitions were then collected into a local feature graph using phase-space 
connectivity as the equivalence relationship. Finally, the notion of a feature graph 
was extended to show how forward projection could be used to produce a more 
restricted from of feature connectivity and thereby incorporate information about 
the consequences of actions. 

Now that we have defined features and put some structure on sets of features, how 
do we build an observer for the current state of the robot using this measurements 
and this structure? Our approach uses two subcomponents. The hrst component 
maps the raw signal into feature models and detects when the signal no longer comes 
from a given model. This is the change detection problem. The second component 
uses the graph and the measurement likelihoods computed from the hrst component 
to approximately compute the best measurement path and therefore the probability 
of each feature instance. These two components are discussed in the next chapter. 



A Contact Feature Observer 



Chapter 6 



The central point of this thesis is to develop a technique for observing the robot's 
contact state. The observer uses knowledge of the c-space and force and position 
measurements to determine where the robot is in a given task. 

The previous chapters discussed the central representational idea of this thesis, 
namely that the statistics of force and position measurements that arise in a given 
task can be represented as a graph of models called the feature graph. Figure 1.1 
shows one example. This graph encodes the prior knowledge available from the ge- 
ometry, the commanded motion, and the local measurement models. This chapter 
shows how to use this representation to derive a statistical decision algorithm that 
observers the contact state. 

Figure 6.1 shows the basic components of the contact state observer. The forces and 
positions are input into a collection of candidate estimators. There is one estimator 
for each hypothesized contact model. The estimators output a measure of match 
between the underlying model and measurements called the log-likelihood. 1 The 
estimators also output a residual process, which conditioned on the model being true, 
is a white noise sequence. The residual processes gets fed into a change detector. The 
change detector monitors the residuals for changes. The change detector computes 
the log-likelihood of departure from the current model and the most likely time 
of departure. Both of these statistics, and the model likelihoods computed by the 
estimators, are sent into a single decision procedure. This procedure uses all these 
process statistics and the feature graph to determine a collection of likely paths for 



^he likelihood of a measurement is the conditional probability or density for the measurement 
given the parameters of the statistical model. The log-likelihood is the logarithm of the likelihood. 
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Figure 6.1: Contact state observer processing. The figure shows the 
basic architecture of the contact state observer processing force and 
position information measured by the robot as it explores the maze. 



the measurements through the feature graph using a beam Viterbi search strategy. 
Since each of these paths terminates in a feature node, the procedure also computes 
an estimate of the current feature node of the robot. Therefore, this process reduces 
the problem of observing the contact state to the problem of observing the current 
contact model. 

This chapter first presents the statistics of the decision problem. Then we specialize 
to the approach indicated in figure 6.1. Recursive estimators and their output statis- 
tics are discussed, and then change detection is presented. We conclude by showing 
how these components can be reassembled to produce the algorithm in figure 1.3. 



6.1 Statistical Observer Formulation 



The observer problem can be formulated in terms of assigning a feature node to 
each measurement given the constraints imposed by the graph. In general, there 
are m feature nodes Mi i = l,...,ra. Each node provides a statistical description 
P;(y(&), ...,y(/)) of part of the measurement process which depends upon some pa- 
rameters 6. There may or may not be a distribution provided for 6. As a series 
of sensor measurements yQ = {y(0), ..., y(n)} are taken, the problem is to gener- 
ate an estimate Mq = {A/"(0), ..., M(n)} for the sequence of nodes from which the 
measurements were produced. Initially, a distribution for the initial feature nodes 
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is given 7r 8 (0) = P(A/"(0) = Mi). Let 7r(0) be a vector representing this collection of 
probabilities. 

The parameters of each node hx the measurement distribution. To complete the 
statistical description, we must determine how these parameters are picked. We will 
use two approaches. Most of the time we are interested in labeling or matching the 
signal against a set of a priori models. In this case, it is useful to determine from 
a set of training data a probability distribution for the parameters for each feature 
instance. This approach assumes that every time the robot enters a given feature 
node, a value for the parameters is chosen from the parameter distribution. The 
value of the parameter then remains fixed until the robot leaves the given feature 
node. 

We associate with every feature node Mf. 

• A parameterized Markov measurement process y(n) ~ p(y(n)|y™I^,, Oi) that 
depends upon at most k samples in the past. 



• 



A parameterized distribution for 6i ~ p(0j-|^). rp i is assumed known and fixed 
based on training data. 



The statistics of the measurement signal up to time n is completely described by 
specifying the sequence of feature nodes which produced the measurements and the 
time at which each node began. 

This model for the observation statistics is one additional level of stochastic abstrac- 
tion than what is commonly used in Hidden Markov Models. It provides a very 
useful additional modeling tool. Experimentally, the feel of stroking a single texture 
tends to be produced by a consistent, constant parameter model for any short stroke. 
However, new motions across the texture, or extended motions, may be produced by 
different parameter values. All of these values can be collected into a single distribu- 
tion for the parameters. Every time the robot enters a given feature, a value of the 
parameters is chosen from the parameter prior associated with the feature. This is 
then used to generated the observations until the robot leaves the feature. 

Note that this creates the possibility of self-transitions. The robot can transition 
from a feature instance back into the same instance, because this models a change in 
the parameter values. Therefore, feature instances, when doing the processing, are 
specified by both the feature model and a starting time. 

Let a 8jt = (Mi 7 t) be a feature instance which specifies the feature node and the 
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Figure 6.2: The computational lattice for the hrst three measurements 
for computing the feature node probability for a graph with two nodes. 



start time for the instance. The lattice in figure 6.2 shows the possible paths for 
the hrst three measurements for a system with two possible feature nodes (a, b). 
The number to the left of the model designator indicates the starting time for each 
model. Horizontal lines are paths which stay in the same model without restarting, 
angled lines indicate self-transitions in the path. Self-transitions model the process 
switching from one set of measurement parameters to a different set within a single 
model. This graphically illustrates that at time n there are ran possible feature 
instances and approximately m n possible paths. The set of all possible states at 
time n will be denoted A(n) and the set of all paths will be denoted S(n). 



The probability of receiving the hrst n measurements given any sequence of feature 
instances s £ S(n) is 

len(s) 



mm 



Urn 



t;+i 



s[ i 



))■ 



(6-1) 



8 = 1 



s(i) is the z th feature instance in the sequence. The sequence measurement distribu- 
tion is computed from the single measurement distribution 



p(y!; +1 l*(0) 
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(6.2) 



3=ti 



where we take ti +1 = n if the feature instance s(i) is the last one in the sequence. 
The probability of any feature path, given the measurements, can be computed from 
these terms by applying Bayes theorem 
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(6.3) 
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The probability of a sequence of measurements is much easier to compute if we hrst 
whiten the measurements with a recursive estimator. An estimator should produce 
an innovations process v(n) which is a white process that summarizes the statistics 
of the measurement process. For linear predictive models, which we used for the 
temporal signals, the Kalman filter is the correct estimator. For the constraint 
models, we derive a recursive maximum log-likelihood estimator. 

The feature observer must compute an estimate of the probability of being in each 
of the feature nodes at time n given the measurements. This probability is the sum 
of the path probabilities over all paths which reach the given node at time n 

P(Af(k)\yr) = £ PHy?). (6.4) 

se<S(n):s(len(s))=AA(fc) 

Evaluating this directly is computationally hopeless because there are an exponen- 
tial number of paths. If transition probabilities between the elements of A(n) and 
A(n-\-l) are available, it is possible to form a recursive computation which uses order 
nm computations for the n th step. Transition probabilities are required, because the 
computation requires a summation over all paths which reach a given feature in- 
stance. The transition probabilities are needed to make this summation meaningful. 
Unfortunately, these are not readily available in our problem. 

Alternatively, the optimal path terminating in any element of A(n) can also be 
determined with order nm computations using a form of the Viterbi algorithm, a form 
of dynamic programming, without transition probabilities. The standard Viterbi 
algorithm was developed for HMM models and does require transition probabilities. 
Let <S max (n) be the set of nm paths which maximize the probability to each of the 
elements of A(n). The estimate of the feature probability 

P(A r (/g)lv") ~ ^s6«S max ( ra ):s(len(s))=AA(fc) f (3|y x ) 

Norm 

where Norm is a normalizing constant, can be computed from the available infor- 
mation. Furthermore, this approach can be further computationally bounded for 
real-time calculations by incorporating a change detection algorithm. 

The change detection approach, will compute an estimate of the best path by testing 
each current best path for deviations from the current feature instance. When a 
change is detected, alternative paths starting from an estimated change time are 
expanded and compared to all the current best paths. Only the best path to each 
element of A(n) is kept. The computations can be further bounded by ranking the 
paths in <S max (n) and keeping only enough paths to ensure keeping the overall best 
path with high probability. 
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The change detection approach relies on the empirical observation that changes in 
the feature instance are generally well separated in time. It further relies on the 
fact that the change test is a renewal process. Therefore, the estimate of the change 
time does not drift from the current estimate unless the test resets. Therefore, the 
observer will never expand more than one path for a single detected change even if 
the observer does not immediately decide to change its best path estimate. 

The sections 6.3 and 6.3.1 discuss the estimator and change detector in general. 
Finally, a formal definition of the observer is given in 6.4. 



6.2 Relationship to Other Detection Problems 



Our approach is related to techniques that have been applied in speech processing, 
failure detection in dynamic systems, and modal change detection for structures. 

In segment based approaches to computer perception of speech, the hrst step is to 
roughly segment the observered signal into phonemes. [Andre- Obrecht, 1988] looked 
at segmenting the speech signal using an autoregressive model for the measurements 
and three different change detection algorithms. The change detection algorithms 
are similar to the one discussed here. 

Both our detector and the work of Andre- Obrecht is based on a number of pa- 
pers by Basseville and Benveniste [Basseville and Benveniste, 1983, Basseville, 1986, 
Basseville et al, 1986, Basseville et al, 1987, Basseville, 1988, Benveniste et al, 
1987]. These works have been applied to segmentation of EEG, ECG, speech, and 
geophysical signals. The best reference is the collection of papers in [Basseville and 
Benveniste, 1986]. 

Their works are all related to the sequential likelihood ratio test, originally developed 
by [Wald, 1947], and the generalized likelihood ratio test (GLR). The GLR test 
was applied to changed failure detection for linear dynamic systems in [Willsky and 
Jones, 1976, Willsky, 1976, Chien and Adams, 1976, Tanaka and Muller, 1990] and 
to detection of incidents on freeways in [Willsky et al., 1980]. Optimality of the GLR 
test has been investigated under many different conditions, one useful reference is 
[Zeitouni et al., 1992]. An alternative test, which can be computed sequentially, is 
developed in [Hall, 1985]. 
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Our path scoring algorithm is based on the Viterbi algorithm for Hidden Markov 
Models (HMM). The major difference between our representation and HMM, is that 
the HMM model assumes that the measurements are independent conditioned on 
the state in the Markov graph. Our model says that the measurements come from a 
measurement model, which can be whitened with an estimator, conditioned on the 
state in the graph. This allows for correlated measurements within each state in the 
graph. The best tutorial on the HMM model is in [Rabiner, 1989]. The HMM model 
was applied to contact perception in [Hannaford and Lee, 1991]. An approach similar 
to ours for dynamic systems is presented in [Tugnait and Haddad, 1979]. Segmental 
approaches to understanding speech [Goldenthal, 1994] also model the correlation in 
the speech signal. 



6.3 Estimating the Current Feature Parameters 



The hrst step in developing a feature observer, is to develop an estimator for the 
chosen feature model. The temporal models used in this thesis were linear predictive 
models, so the Kalman filter is the appropriate estimator. The constraint models use 
a combination of a maximum likelihood estimator for the parameters, and a Kalman 
filter prediction of the measurements. In either case, we assume that it is possible to 
develop an estimator which produces estimates via 

0(n) = 6(n - I) + 7 H(0(n - l),y(n)) (6.6) 

and measurement innovations via 

i/(n)=y(n)-E[y(n)|0(n-l)]. (6.7) 

The generated innovations process will be assumed to be asymptotically white and 
normal. The covariance of the innovations are either provided or estimated on-line 
using 

V u (n) = V u {n - f) + -{u{n)u{n) T - V u {n - f )). (6.8) 

n 

The innovations process has the property that 

p(y?K) = pKK). (6.9) 

Furthermore, because the innovations are white, the log of the probability of receiving 
the measurements (the log-likelihood) given the feature is 

logp(y t f ' +1 |a / , t ) = £>gp(i/(j)|/). (6.10) 

3=t 
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Thus the innovations reduce the problem to summing up the log probabilities of a 
sequence of measurements. Unfortunately if the variance of the innovations must 
be estimated, this computation cannot be done recursively. Instead, the current 
estimate of the variance must be used to compute the log likelihood of the current 
innovation. For changes which occur sufficiently separated in time, this approach 
will be sufficient. 



6.3.1 Testing for Changes in the Model 



The goal of the observer is to estimate the feature instance path given the graph. 
Dynamic programming can produce the optimal path up to the current time, but 
at the cost of a linearly increasing number of models and computations with each 
new measurement. In order to limit the computational cost, the observer orders 
the possible paths by probability. Then only enough paths to capture, with high 
probability, the best future path are tracked. To further limit the computation, the 
observer only branches and produces new path possibilities when a currently tracked 
path has most likely undergone a change to a new model. Detecting these changes 
is covered in this section. 

The change detector takes the form of a sequential hypothesis test on the innovations 
process produced by the feature parameter estimation algorithm. The area of sequen- 
tial hypothesis test for detecting jump changes in statistical processes has been an ac- 
tive area of research in statistics and signal processing since its initial development by 
Wald [Wald, 1947]. A mathematical review is given by Siegmund [Siegmund, 1985]. 
There have been a number of important results during the last decade [Willsky, 1976, 
Basseville, 1988, Benveniste et al., 1987]. These methods are relevant to any signal 
processing task which can be modeled as a stochastic measurement process on an 
underlying system which undergoes discontinuous changes. The methods are particu- 
larly useful when accurate and rapid decisions about the time of change are required. 
This includes edge detection, continuous speech segmentation, and contact sensing. 

In sequential hypothesis testing it is assumed that the time for the algorithm to 
detect a transition is short compared to the holding time before a second transition. 
Therefore it is assumed: 1) that transitions can be detected by considering only the 
data, and 2) only one transition from this hypothesis needs to be considered. 

In order to apply the approach we need two hypotheses for the innovations process. 
The hrst hypothesis, the null hypothesis H , is that the current feature model is 
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correct. Under this assumption the test statistic 

i/(n) T V(n)" 1 i/(n) (6.11) 

is approximately x 2 ( m ) distributed for a vector measurement process of size m, be- 
cause v(n) is asymptotically normal and V(n) converges to the covariance of v. If 
the process changes, both the mean and variance of v might change. For our pur- 
poses, the simple alternative hypothesis that the change in v is reflected in a change 
in magnitude of the covariance seems to suffice. More sensitive tests which involve 
estimating the direction of change can also be applied [Eberman and Salisbury, 1994]. 
Therefore, our alternative hypothesis Hi is that v{n) ~ N(0, q\(n)) } where q is the 
user selected change in variance. 

Given these definitions we want to test the innovations process for a possible change 
at time r between 1 and n. We form the likelihood ratio between the hypothesis that 
the process was generated by H from time to time r — 1 and then in Hi from time 
r to n versus the hypothesis that the process was always in H . 

Because the innovations process is white, the likelihood ratio is 

^-o> = pw "S:t ihi) (6.i2) 

p{v \ilo) 

= U*«m- (6 - 13) 

That is whiteness, or independence, implies that the likelihood is the product of 
simple terms. 

To simplify the calculations let jo(t) = l°g(p(i/(t)|H )), Ji(t) = log(p(i/(t)|Hi)), 
^■oi(t) = 7i(0 — 70(^)5 an d Sl(0, 1) = Y?t=k ^oi{t)- Simplifying the expressions for 
the required change in covariance test shows that 

Soi(t) = l/2(-mlog( ? ) + (1 - l/ g )i/(t) T V(t)-V(t)). (6.14) 



The decision function, DF, for a change from state to state 1 is 

DF(0,l,v%)= maxlo gj L(0,l,r,^) (6.15) 

r£[0,n\ 



which results in the binary rule 



Hi 

DF(0,l,i,%)>T 2 . (6.16) 

Ho 
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This rule says that Hi will be chosen as the new state if DF(0 } 1, i/q) becomes larger 
than T 2 , otherwise H will be maintained as the current hypothesis. T 2 is the decision 
threshold and is a design parameter that controls the essential trade-off between the 
detection delay and the false alarm rate. Increasing T 2 increases the detection delay 
and decreases the false alarm rate. For a process with multiple changes a large 
detection delay could lead to missing changes. 

The decision function is equivalent to the Page-Hinkley (PH) cumulative sum stop- 
ping rule 

DF(0,l,vZ) = ^(0,1)- mm ^(0,1). (6.17) 

0<j<n 

This test minimizes the time taken to reach decision Hi over all tests that have the 
same false alarm rate [Siegmund, 1985]. Further, it is easily computed recursively by 

DF(0, I, u n ) = max(0, DF(0, I, u^ 1 ) + E 01 (nj). (6.18) 

This is essentially integration with reseting at 0. 

Intuitively, the value of q is chosen such that as long as H is correct the integration 
will on average drift negatively. Reseting the integration to zero then always restarts 
the process at zero. When a change occurs, the process integrates in the positive 
direction and eventually crosses the decision threshold. In some sense this test is 
thresholding a lowpass filtered signal with the lowest possible frequency filter. 

There are two important characteristics of any hypothesis testing procedure: I) the 
false alarm rate, 2) the delay to detection. The earliest time at which the decision 
function exceeds the threshold, given that the system is still in state 0, is the false 
alarm time tf = inf(n : DF(0,1,i/q) > T 2 ) which has distribution Pi?^(n). The 
probability of no alarm at time n is Pjva(^) = 1 — Pfa(^)- The asymptotic false 
alarm rate is defined to be f = 1 — lim„_ >00 N f^ n ) . This reflects the rate at which 

•> n >°° P NA (n-l) 

false alarms will occur over the long-term. In contrast, the delay to detection is 
a transient performance measure. The delay to detection, given that a change to 
state I occurred at time 0, is tr> = inf(n : DF(0,1,i/q) > T 2 |Initial state isHi). 
The distribution of tr> is P.o(n) and its expected value is tjj = YHtLo^ D{t)- Both 
statistics are controlled by T which is a design parameter. Increasing T decreases 
the false alarm rate and increases the time to detection. Determing both of these 
relationships requires solving a hrst passage problem. Closed form solutions to this 
type of problem are rare and difficult to derive. Approximations for some simple 
cases are discussed in [Eberman and Salisbury, 1994]. 
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In particular, the Page-Hinkley (PH) test can be compared to the very popular 
method of filtering followed by thresholding. Both approaches take exactly the same 
amount of computation, but as the following figures show the PH test provides 
superior performance. A comparison was done for tests which have the same false 
alarm rates. 

The asymptotic false alarm rate PH f and time to detection PH io for the Page-Hinkley 
test can be approximated by applying Wald's identity and approximations [Sieg- 
mund, 1985]. The results are 

PHT _ I X 2 rp2 



If ~ ^ -T 2 -I|//3 

a 



PH t D « (e- T + T 2 - l)/# 
where 



# = /log 



pi(0 



M()d(- 



MO. 

Since the false alarms are the interarrival times of a Bernoulli process they are geo- 
metrically distributed. Therefore the asymptotic false alarm rate is 

FH f _ 1 



PH t, 



For the change in mean between two Gaussian processes with the same standard 
deviations <r, /3 8 - is 

A = 1/2(V 



G 



A plot of the trade-off between the time to detection, id, and the time to false alarm, if 
is called the receiver operating characteristic (ROC). It is a function of the signal- 
to-noise ratio s = — -. Graph 6.3 shows the value of id and log 10 if parameterized 
by T for a fixed value of s. The ROC for this test is shown in figure 6.3 for s = 
0.5,1.0,1.5,2.0. Both the mean time to a false alarm and detection increase with 
increasing threshold. At a fixed false alarm time, an increase in the signal-to-noise 
ratio will decrease the time to detection. 

The performance of the alternative test of lowpass filtering followed by thresholding 
can be bounded using the following asymptotic approximation derived by Hall [Hall, 
1985]. The approximations are valid in the limit of an increasing threshold and short 
sampling time. Consider a filter realized by a stable, linear, time invariant vector 
process x 

x(k + I) = Ax(k) + w(k + I) + Ajiu-^k - r) 
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ROC for Page-Hinkley Test 




Figure 6.3: Receiver operating characteristic (ROC) of Page-Hinkley 
test between two Gaussian distributions with different means and the 
same variance as a function of the signal to noise ratio s = — -. The 
log 10 (tj) is shown as a function of the mean time to detection tj, for 
s =0.5, f.O, 1.5, and 2.0. 
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driven by a white, zero-mean, Gaussian noise w(k) with noise intensity Q. A change 
of size A/i is applied by the unit step u_i at time r. The covariance of x is a 
steady-state solution to the discrete Lyapunov equation and satisfies S = ASA T + 
Q. The decision function is DF(k) = x T (k)S~ 1 x(k). In principle it is possible to 
determine Pfa(&) by propagating the density for x(k) } p(x, k) } forward in time and 
then integrating over the decision region. The propagation equation is 

p(x,k + l)= / p w (x - A()p((,k)d( 

JD 

where D = {x : DF(k) < T 2 }. Then Pfa(&) is given by 



Pfa(^) = 1 — / p(u,k)du. 

JD 

Unfortunately there are no closed form solutions to this problem. However by treat- 
ing the discrete system as a sampling of a continuous system, an approximation valid 
for large k can be determined. Using this approximation, the steady state false alarm 
rate / is found to be asymptotically bounded by 

where p is the dimension of i. In the case of a first-order lag filter x(k + I) = 
ax(k) + w(k) } the bound is 

/o < 1 - exp (yV21n(a)Texp- T2 / 2 (I - V^ 2 )) • 

This is the bound for x 2 / S > T. The PH test is equivalent to X/S 1 ' 2 > T which 
has a false-alarm rate bounded by fo/2. 

To approximate P_d(&) note that DF(k) is a noncentral chi-squared random variable 
with p degrees of freedom and noncentrality parameter S 2 (k) = x T (k)S~ 1 x(k) [An- 
derson, 1984]. The process mean x satisfies 

x(k + I) = Ax(k) + Aji 

with initial condition x{0) = for a change in mean of A//, where we have assumed 
for simplicity r = 0. If the cumulative noncentral chi-square distribution of DF at 
value T 2 is denoted by F(F 2 } 6 2 } p) } then P_d(&) is bounded by 

P D (k)>l-F(F 2 ,6 2 ,p) 

which can be computed numerically or approximated. 
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For a scalar, first-order lag-filter, the ROC can be computed as a function of the 
signal-to-noise ratio s as in the PH test. In this case, the values of id and log 10 if are 
parameterized by a. The optimal threshold for the test is -j^- where S = "'' cr 2 , 



45 "-^-^ ^ (1 + a) 



(1-a) 




This gives a threshold of T = ( | J ( 1+ °{ - With the one-sided test, an approximation 
for P_d(&) is simply the probability of drawing a value greater that A///2 from a 
Gaussian random sample which has mean x(k) and variance S, given that the test 
has not already terminated. The probability of terminating at time k given that the 
test has not already terminated is 



F(k) = 1 



The probability of terminating at time k is then given by the recursion 

P D (0) = F(0) 

P D (k) = F(k)(l-P D (k-l)). 

This gives an underestimate of the termination time. An overestimate is given by 
the rise time for x(k) to A///2. Figure 6.5 shows the logarithm of if as a function 
of ij, for a signal-to-noise ratio of s = 0.5, 1.0, 1.5, and 2 computed using these two 
approximations. The curve for s = 0.5 has been cut short, because the approximation 
is not valid for small id- 

An examination of both figures shows that the performance is better for the Page- 
Hinkley stopping rule for all signal-to-noise ratios greater than 0.5. With a signal-to- 
noise ratio of 0.5 the figures indicate that filtering performs better. This is most likely 
do to the approximations used in computing these curves. The ROC curve for the 
filtering approach is only an upper bound and the true performance is probably lower. 
The ROC curve for the Page-Hinkley test is also computed from an approximation. 
According to the theory the Page-Hinkley test will always perform better at all 
signal-to-noise ratios. 

Figure 6.6 indicates that the lowpass filter approach has a longer delay to detection 
compared to the PH test when they have the same false alarm rate. The test shown 
in figure 6.4 will signal an alarm on average every 6 X 10 6 samples and the change 
will be detected after 28 samples. To get equivalent performance from the lowpass 
filter, a must equal 0.98. With this value, the estimate of ip is 29.5 and the rise 
time is 34.5. These results demonstrate that the PH test gives an improvement in 
performance without an increase in computational cost. In addition, an estimate of 
the change time is possible by storing a single number. 
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Figure 6.4: Behavior of the Page-Hinkley stopping rule to a simulated 
change in mean at tick 126 for a Gaussian process. Signal has standard 
deviation of 1 before and after the change, and mean of 1.0 after the 
change. Change is detected with a threshold of 15 at tick 149. The 
estimate of the time of change is the last time the test equals zero 
which is at tick 128. 



112 



Chapter 6: A Contact Feature Observer 



8 
7 




ROC for Lowpass Filter Test 




2.0 


7 7 

/ / 
/ ls / 








/ 10 / 




6 


/ 




- 


5 


/ 




- 


4 


/ / 




0.5 ^^^ 


3 


/ / 




- 



10 15 20 25 30 35 40 

td 



Figure 6.5: Receiver operating characteristic (ROC) of first order lag 
filter test with threshold between two Gaussian distributions with dif- 
ferent means and the same variance as a function of the signal to noise 
ratio s = — -. The log 10 (tj) is shown as a function of the mean time to 
detection id- 
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Figure 6.6: Lowpass filter of x(n + f ) = 0.98x(n) + 0.02j/(n + f ) on the 
same signal as figure 6-3. The threshold is 0.50. The change is detected 
at 153. This is a slower response then the response for the PH test. 
Further, an estimate of the change time is not computed. 
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6.4 Computing the Feature Probabilities 



This section summarizes the local feature observer based on all the preceding back- 
ground. The observer is given a local feature graph Q consisting of a collection of 
nodes M and edges £. An initial probability distribution over the graph for the cur- 
rent contact state 7r(0), and a probability pruning threshold "Pprune are also provided. 
Finally, a change detection threshold T 2 and a change accumulation magnitude q is 
provided. 

A Markov model is associated with every graph node n for the measurement parame- 
terized by a set of known, fixed parameters tj) n . The values of these parameters were 
determined by estimating their values using previously segmented training data. 

In order to initialize the observations process, the observer: 

1. Creates a buffer to store past measurements. 

2. Sorts the nodes by their initial probabilities from largest to smallest. 

3. Selects nodes from this sorted list until the sum of the probabilities of all the 
nodes is greater than "Pprune- 

4. Creates for each selected node, an observer for the measurement model and a 
change detector for the residual process. The threshold for the detector is set 
to T 2 and the size of the alternative hypothesis is set to q. 

5. Initializes the path log-likelihood of each model to the logarithm of the model's 
prior probability. 

6. Creates a buffer for each measurement model to store past values of the path 
log-likelihood for that model. 

As each new measurement arrives, the observer: 



1. Stores the value of the measurement in the measurement buffer. 

2. Updates all the current models and change detectors using the new measure- 
ment. 

3. Accumulates the log-likelihood of each residual in order to track the path log- 
likelihood. 
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4. Stores the path log-likelihoods for each model in each model's path log-likelihood 
buffer. 

5. Records the maximum time in the past, over all current models, at which a 
change could have occurred. 



If a change in any model is detected, then for every model in which a change is 
detected, the observer: 



1. Creates a new estimator for every possible model to which a transition can 
occur given the edges £ in the feature graph, and for which the same model 
and starting time are not already being tracked. 

2. If an estimator with the same starting time and model is being computed, 
the initial path log-likelihood of the estimator is compared to the new spawn- 
ing model. The current estimator is set to have the higher initial path log- 
likelihood. This implements the maximization step of the path tracking algo- 
rithm. 

3. If the new model and starting time are not being tracked, the path log- 
likelihood of the new model is initialized to the path log-likelihood of the 
spawning model at the change time, and the measurements from the change 
time to the current time are incorporated into the new model. 

4. Resets the change test statistic for the spawning model. 



The observer now has a new collection of distinct paths. These paths are sorted on 
their path log-likelihoods. The log-likelihoods are then normalized and turned into 
probabilities. Enough of the new paths are then kept so that probability mass "Pprune 
is retained. Finally, the probabilities are renormalized. 

If a change is not detected in any current path, the observer shortens all the current 
buffers to their minimum possible lengths. For the measurement buffer, this is the 
minimum change time over all models. For the path log-likelihood buffers, it is 
the minimum change time for that path. The observer then computes the relative 
probability of each path by renormalizing the log-likelihoods. 

Finally, for both change and no change, the probability of each feature is computed 
by summing path probabilities over paths that are currently in that feature. The 
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resulting observer estimates the probability of being in each node in the feature graph 
by computing estimates of the best path for all the measurements. Computations 
are minimized by incorporating a set of change detectors on each active model. The 
detectors monitor the innovations produced by the parameter estimators and signal 
a change, or an event, when a cumulative sum stopping rule crosses a threshold. 



6.5 Conclusion 



This chapter presented a sequential decision approach to observing manipulation 
state. The manipulation state was encoded by a feature graph and the observer 
tracked the feature state by determining the most likely sequence of measurement 
models given by the features. 

Change detection was used to control the expansion of the path likelihoods. It is 
critical in manipulation tasks that changes in the contact conditions be rapidly and 
robustly detected. Change detection theory provides a general tool for designed 
algorithms for this task. The probabilities of the paths were then related to the 
probabilities of each feature node. Since each feature node corresponds to a region 
of phase-space, the system can determine the probability that the robot occupies a 
region of phase-space. Thus, the robot can track its motion in a task. 



Time Series Models 



Chapter 7 



This chapter looks at temporal models which capture properties of the force (or 
strains) signal considered purely as a time series. The force signal is strongly affected 
by textural properties, frictional stick-slip, and contact transients. These effects 
appear in the time series and affect the high frequency range of the force sensor. 
Time series models of the derivative of the force are useful models for these effects. 

For example, figure 7.1 shows a spectrogram of an impact event. The impact results 
in an increase in energy at all frequencies locally around the event, and a persistent 
residual vibration at the sensor's natural frequency. It is important to isolate both 
the beginning and end of impact events in manipulation sensing. 

Depending on the stiffness of the contacting materials, the beginning is usually easy 
to sense. For soft materials, the peak force is easy to sense but the beginning may 
actually be hard to sense. For hard materials, the impact rise time is so short that 
the beginning and peak are essentially the same. Sensing both the beginning and 
the peak is nice, because the rise-time is a good indication of the stiffness of the 
impacted material. 

The end of impacts is always difficult to determine because the sensor and the robot 
vibrate after each contact. This vibration slowly dies away and an impact event is 
over when the vibration has sufficiently decayed. The end of the vibration needs to 
be sensed, so that a constraint estimator can be started for low frequency forces. 
The constraint estimator cannot be run while the sensor is in an impact event, and 
we would like it to run as soon as possible so that the robot can use the estimated 
constraints for force control. Uniform textures also produce temporal patterns that 
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Figure 7.1: Spectrogram of an impact event. The upper figure shows a 
single strain signal with an impact event. The bottom figure shows a 
contour plot of the energy in frequencies from 200-1350 Hz as a func- 
tion of time. The signal was sampled at 2700 Hz. Sixty-four points 
windowed with a Hamming window were used for each fast Fourier 
transform (FFT). The FFT was computed for every new data point. 
Note the broad frequency band that occurs at an impact and the short 
time scale of this event. 
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can be useful for identifying the contact location and contact type. We used time 
series models to model steady vibration levels, impacts and uniform textures. The 
raw strain signal and the derivative of the strain signal are used in the modeling. 

Two basic problems in temporal modeling were examined during the course of this 
research: signal labeling and signal description. Signal labeling is matching the mea- 
surements against a set of predefined models. The predefined models are created 
off-line from hand segmented training data. Signal description is regression against 
a set of model classes so as to best describe the signal over the classes. This chapter 
discusses labeling. Signal segmentation and description is discussed in previous pa- 
pers [Eberman and Salisbury, 1993, Eberman and Salisbury, 1994]. A simple change 
detection based segmentation procedure was used to produce the training samples 
for labeling. 

Both problems require estimators to compute the likelihood of a model. In la- 
beling, estimators incorporate prior knowledge of parameters in computing model 
likelihoods. We used a square-root implementation of the Kalman filter for label- 
ing because it is fast and numerically robust. The standard implementation of the 
square-root filter was slightly modified for some of the models to produce an orthog- 
onal regressor square-root filter. Appendix A. 2, on filtering and estimation, discusses 
the detailed implementation of the filters. 

The experiments discussed in this chapter show: 



• That autoregressive models of the strain signal produce segmentation bound- 
aries that make sense from the physics of the motion. 



• That, at least for this sensor, most of the information for labeling textures is 
in the vibration energy. 



• That the logarithm of the energy in the time derivative of strain is a good 
measure of vibration. Impact events can be isolated from the signal in this 
form at lower frequencies then in the raw strain signal. 



• Context in the form of the correct model sequence, for a given measurement 
history, significantly helps in recognition. 
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7.1 Signal Measurements and Models 



For temporal modeling the raw strain signals and generalized forces measured by 
a force/torque sensor provide a basis for modeling. We used the strain signals for 
temporal modeling because they are directly available and equivalent, up to linear 
transformations, to the generalized forces. 

The strains y(n) were sampled at 1000 Hz, and the hrst order difference y<f(ra) = 
y(n) — y(n — 1) was computed. A high sampling rate is required to accurately 
capture the rapid vibration of short time events like impacts. The raw signals were 
then lowpass filtered with a second order filter at 125 Hz and then subsampled at 
4:1 to produce a 250 Hz signal, y;. Filtering in this way empirically isolated the 
strain signal that was due to contact from the vibration signal. The contact induced 
strains are all low frequency in nature because they are caused by the motions of the 
robot. We filter and decimate in this ratio to maintain the flat, or white, spectrum 
of the measurement error. The change detection algorithm does not work as well 
with colored noise. 

In addition to this signal, the logarithm of the energy in the derivative of the strain 
signal was computed. This high energy signal is formed by the filter 

y h (n) = log(|y d (n)|). 

The logarithm signal was then lowpass filtered by averaging blocks of 8 points to- 
gether and then sub-sampling at 4:1 again producing a 250 Hz signal with a flat noise 
spectrum. The averaging reduces noise in the signal. The effect of the logarithmic 
transformation and filtering is to estimate the envelope of the energy signal. 

This can be seen by writing y^ as the product of two terms 

Yd{n) = q(n)v(n) 
where q(n) is a slowly varying envelope and v is a white noise process. Expanding 

log (yl M) y ields 

log(ydM) = log(?W 2 ) + logO(n) 2 ). 

An analysis of the spectrum of these two processes shows that log(q(n) 2 ) has a low 
frequency spectrum and log(z/(n) 2 ) is broad spectrum. Therefore, lowpass filtering 
removes some of the noise and emphasizes the envelope. This approach is a form of 
homomorphic filtering [Oppenheim and Flanagan, 1989]. 
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Figure 7.2: Fitting a stationary texture signal with an autoregressive 
process. The top hgure shows one of the measured strain signals, and 
the bottom hgure shows the autocorrelation of the residual after esti- 
mating four AR parameters. The spiked form of the autocorrelation 
shows that the AR model produces fairly white residuals and is a good 
fit to the measurements. 



We used linear predictor coding (LPC) for both measurement processes. Each signal 
was treated as independent. Autoregressive (AR) models with and without a mean 
were used for models of textures and steady motions for both the raw strain signals 
and the high energy signal. The AR model is 

p 

y( n ) = Yl a iy( n -«) + /" + u ( n ) 

i 

where v{n) is a white, Gaussian processes, {a,-} are the autoregressive parameters, 
and fi is the forcing mean. Figure 7.2 shows an example AR fit to a stationary 
strain signal. The feature vectors were formed from the linear predictor coefficients 
generated by this model and the parameter order {//, ai, a 2 , ...}. 



The dominant characteristic of impacts is the sharp rise of the event and then expo- 
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nential decay of the vibration envelope. If the impact is caused by gaining a contact, 
the strain signal will also show a change in mean. Intermittent impacts will not show 
a change in mean. The effects of the change in mean are eliminated by looking for 
impacts in the difference signal y^. 

Figure 7.3 shows the difference formed from one of the strain signals from an impact 
and the high energy signal computed from this strain in dotted lines. An envelope 
produced by lowpass filtering the log signal is also shown in solid in both traces. 
The plots clearly show that the effect of filtering is to produce an estimate of the 
envelope. This envelope estimate can then be interpreted at the lower processing rate 
of 250 Hz. The lower logarithm plot also shows that a step rise in energy followed by 
a linear decay provided a good model of the signal in the logarithmic domain. The 
appropriate model is 

y(n) = fi + an + v(n) 

for a signal beginning at time 0. Again the feature vectors were formed from the 
linear predictor coefficients generated by this model and the parameter order {//, a}. 



7.2 Labeling Stationary Models 



Autoregressive models provide a good model for stationary strain and force signals. 
The feature parameters are the linear predictor coefficients generated by the model 
and the variance of the driving noise. In order to test the labeling performance of 
this feature, a simple two texture experiment was performed. 

The PHANToM was used to stroke the sample in the x direction under open-loop 
force control. The complete motion command took 5 seconds. Data was recorded 
for almost 7 seconds. A downward force of 200mN was applied with zero stiffness 
and damping in the vertical direction. Position control was used in the x direction. 
The fingertip position was measured through the PHANToM using encoders on the 
motors. 

The hrst sample (sample A) consisted of a single two inch section of aluminum. One 
inch of the aluminum was sanded crosswise with 80 grit sandpaper. The second 
half was polished. The second sample (sample B) was a polished piece of aluminum 
inlayed into a rough cross-cut piece of wood. Eight trials were performed for each 
sample. A typical set of strain measurements and the x position for the hrst sample 
is shown in figure 7.5. 
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Figure 7.3: Characteristics of an impact processes. The primary char- 
acteristic is the sharp rise in strain followed by vibration within an 
exponentially decaying envelope. The upper figure shows the difference 
signal and an exponential envelope fit from the logarithmic trace. The 
lower figure shows the logarithm of the absolute value of the difference 
signal and an envelope created by lowpass filtering. 
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Figure 7.4: Two-texture labeling experiment. Each sample consisted 
of two textures placed side by side. Two different samples were used in 
the experiments. The surface was stroked with the sensor under force 
control. 



There are 7 distinct phases to the motions which appear in the plot. The hrst phase is 
caused by the sensor resting on the plate. The next impact-like burst is caused by the 
start of the motion command. The sensor moves slightly forward and then is halted 
by friction. The third segment is the slow build up of strain as the position controller 
ramps up the torque commands to overcome contact friction. The fourth segment is 
motion over the rough surface. The fifth segment is motion over the smooth surface. 
The sixth segment, the short fast drop in strain, is caused by the termination of 
the motion command. Upon termination the system switches to a command which 
freezes the robot in its current position, thus the strains decrease. The last section 
is is from the sensor again resting against the surface of the aluminum. 



7.2.1 Data Segmentation 



In order to test the performance of autoregressive models in labeling example signals, 
segmented components were created using a change detector based segmentation. 
After automatically segmenting the data, the pieces that corresponded to steady 
motion over the textures were selected. These were collected together for modeling 
training and testing. For completeness, this subsection summarizes the segmentation 
procedure and empirically shows the effect of the decision threshold on the number 
of segments. 
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Figure 7.5: Typical result from the stroking experiment. The upper 
figure shows the preprocessed strain signals versus time. The lower 
figure shows the global x coordinate and velocity versus time. The 
signal division lines were generated by the segmentation procedure with 
a decision threshold of fOO. 
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The segmentation approach uses the Page-Hinkley cumulative sum test (see chap- 
ter 5) to detect changes in the residuals produced by a parameter estimator. Fig- 
ure 7.6 shows the complete signal processing architecture for segmenting the strains 
or forces using this approach. The estimator is discussed in detail in appendix A. 2. 
The Page-Hinkley test is presented in section 6.3.1. 

After preprocessing, each of the strain signals is run through a square-root param- 
eter estimator. Two autoregressive terms are estimated by the filter. Each of the 
residuals produced by the estimators is then sent to a variance estimation scheme. 
The variance estimator starts on the ninth measurement. The sum of the residuals 
squared divided by their variance estimate is then sent to the Page-Hinkley change 
detector. This statistic is chi-squared with eight freedoms. The change detector 
looks for changes that correspond to a change in covariance of size 2.5. This level 
essentially corresponds to a change that is approximately y/1.5 standard deviations 
away from the estimate of the variance. Experimentally this level was a good balance 
between sensitivity to changes and false alarms. Both increases and decreases in co- 
variance magnitude are tested. The change likelihood is then accumulated using the 
Page-Hinkley rule, and if the change statistic crosses a decision threshold a change is 
indicated. After detecting a change, both the square-root estimator and the variance 
estimators are restarted. 

The performance of the segmentation procedure can be gauged by comparing the 
segmentation points at different decision thresholds. Figure 7.7 shows the effect of 
the decision threshold on the number and position of the detected changes for one 
of the experiments with the hrst sample. Lowering the threshold increases the num- 
ber of boundaries. However, the location of previous boundaries remains relatively 
constant. The number and location of the boundaries is heuristically reasonable for 
most threshold levels. At very low values of the threshold the test detects very small 
changes in the signal. Because the larger boundaries are maintained, these could 
possibly be removed by subsequent post-processing. Finally, note that the boundary 
caused by the change in surface texture appears at all threshold levels. 

Since the test specimen has two distinct surface textures, the boundaries produced 
with A = 200 were used for segmentation. Each experiment was segmented at this 
threshold and two training batches were formed for each test sample. 
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Figure 7.6: Signal processing architecture for on-line segmentation of 
strains and forces using an auto-regressive model. The strain or force 
signal is preprocessed using a low pass filter and decimation. Then a 
square-root parameter estimator is applied to the signal to produce the 
reflection coefficients. The estimator residuals are used in the Page- 
Hinkley cumulative sum test. If this test crosses the decision threshold, 
the estimator is restarted using the next measurement. 
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Figure 7.7: Effect of the decision threshold on the number and loca- 
tion of the segmentation boundaries for autoregressive segmentation. 
The bottom figure shows the preprocessed strains signals from one ex- 
periment using sample A. The top figure shows the placement of the 
boundaries for different levels of the decision threshold. 
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7.2.2 Batch Performance 



A leave-one-out procedure was used to gauge the batch performance of the autore- 
gressive models. In this procedure one example is left out of the example set. The 
models are then trained on the remaining examples. The left out example is then 
tested against the models. It is marked either correctly or incorrectly by the labeling 
process. Each sample is then left out in turn. The total performance is computed 
from the performance over all samples. 

For sample A, the 8 training segments were subdivided into sections of length 100. 
This produced 67 blocks of data where each block consisted of the 8 strain measure- 
ments. For every individual strain signal a second order autoregressive model was 
estimated and the two LPC coefficients and the driving variance was recorded. This 
produced a 24 dimensional feature space consisting of 8 independent groups of three 
features. 

The decision procedure is a binary hypotheses test between two cases. The autore- 
gressive measurement model for each strain and each case is of the form 

Y = [Y(-l),Y(-2)]0 + i/ (7.1) 

Y = .40 + 1/ (7.2) 

where Y is the complete measurement vector for a single strain, Y(— i) is the z th 
lagged version of the vector, are the AR coefficients and v is the noise term. 
The LPC measurement model is formed by decomposing A into QR using the QR 
decomposition. After decomposition the model is 

Y = QS 1/2 k + i/ (7.3) 

where S is the diagonal part of R and k is the vector of LPC coefficients. 

Both v and k were treated as normal random variables. For each block of data, 
the mean and covariance of k was estimated from the examples with the test block 
removed. The variance of the driving noise, Vj/,-, was also estimated. Since Y is 
the sum of two independent normal random variables, its distribution is also normal. 
The mean and covariance of Y are 

E[Y] = QS 1/2 E[k] (7.4) 

V[Y] = QS 1 / 2 V[k](QS 1 / 2 ) T + Vi/. (7.5) 

This distribution was used in a likelihood based procedure which resulted in 100% 
correct marking of the examples. 
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Figure 7.8: Histogram of the logarithm of the energy in the time deriva- 
tive of the strains. The relative separation of the empirical densities is 
confirmed by the normal fit to the densities. 



The relative contribution to this success of the LPC coefficients versus the magnitude 
of the noise is not clear. It is possible to achieve similar levels of success by testing just 
the magnitude of the vibration energy. Figure 7.8 shows the empirical and estimated 
densities of the logarithm of the energy in the time difference of the strains. This 
is the signal y^. Based on the normal fit to the histogram an error probability of 
5 X f0~ 4 would be expected. 

A similar series of computations was performed for experiment B (the wood and 
aluminum block). In this experiment, the 8 training segments were divided into 
blocks based on the 6 wrench measurements. Each individual wrench signal was 
used to generate LPC feature vectors. Each signal was again treated as independent 
and the equivalent binary hypothesis test was performed. In this case, the results 
are summarized by the confusion matrix below. 



Correct 
Hypothesis 



Chosen Hypothesis 





Wood 


Al. 


Wood 


0.8125 


0.1875 


Al. 


0.0323 


0.9677 



Again much of the discrimination information is contained in the vibration magni- 
tude. An error probability of 0.225 can be predicted from a normal fit to the average 
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energy in y^. This is very close to the measured rate of 0.2198. 

Based on these experiments we conclude that, at least for this type of discrimina- 
tion the AR models provide only marginally more information than the information 
available from the log-energy Therefore, if we are only interested in discrimination 
the log-energy signal is sufficient. However, if other processing will be done on the 
strains, perhaps in conjunction with position measurements, the AR models provide 
a fast, unstructured, method of segmentation. In our experiments the boundaries 
found by this technique corresponded to interesting physical events so the segments 
produced should be useful for additional processing. Furthermore, segmentation on 
the strain rather than the log-energy should produce better results because there are 
more degrees-of- freedom to fit in the estimates. 



7.3 Recognition using High Frequency Models 



The high frequency vibration signal provided by y^ clearly is an important signal 
for understanding manipulation. The last section showed that in two forced choice 
experiments quite good discrimination could be achieved purely on the mean value 
of this signal. This section extends this work by examining segmentation and la- 
beling for the high frequency signal. Stationary models were represented by LPC 
coefficients and the driving noise variance. In addition, nonstationary impact events 
are represented by linear decay models. 

Real-time recognition involves simultaneous segmentation and labeling of the data, 
against prior models, as the data is received. Recognition of high frequency events is 
important for isolating the beginning and end of impacts and as a secondary source 
of information about textures and motions. As an experimental demonstration, we 
looked at recognition using y^ against four hypotheses: 



• H : The PHANToM is stationary and not touching anything. 

• Hi: The PHANToM is moving and not touching anything. 

• H 2 : The sensor is being pressed against a surface, but is not moving along the 
surface. 

• H 3 : The sensor has been hit and signal represents the falling edge of an impact. 
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These hypotheses cover events generated by guarded moves from free space. 

Fifteen training runs were performed to calibrate the four models. The hrst five 
moved the robot from (0,0,0) to (0,0.7,0.7) in 0.5 seconds. The acceleration of the 
desired trajectory is a sinusoid, so the trajectory is smooth. The sensor hit and came 
to rest against a block at the end of its motion. The second group of five moved 
the robot (0,0,0) to (0,0.7,0.7) in 0.5 seconds with no impact at the end. The last 
group moved the robot from (0,0,0) to (1,0,0) in 2.0 seconds with an impact and 
contact at the end. 

The test set consisted of 12 runs in blocks of three. The hrst block moved the robot 
from (0, 0, 0) to (0.0, 0.7, 0.7) in 0.5, 1.0, and 2.0 seconds respectively with an impact 
and contact at the end. The second block performed the same motion without an 
impact and contact at the end. The third block moved from (0,0,0) to (1,0,0) in 0.5, 
1.0, and 2.0 seconds respectively with an impact and contact at the end. The last 
block executed the same motion without an impact or contact. 

The data from the training runs was hand segmented, and collected into examples 
for the four hypotheses. The data from the three stationary hypotheses were addi- 
tionally segmented into units of 100 measurements. For each stationary example, the 
LPC coefficients generated by a mean and a single AR coefficient was determined. In 
addition, the driving noise variance was estimated. Figure 7.9 shows a two dimen- 
sional scatter plot for the two LPC coefficients over the three stationary models. An 
approximate estimate of the confusion matrix can be determined empirically from a 
normal fit to this scatter plot. The result is given below. The actual confusions are 
likely to be worse because the data does not appear to match the normal assumption 
well, but we still expect labeling performance between 80 and 90 %. 



Correct 
Hypothesis 



Chosen Hypothesis 





HO 


HI 


H2 


HO 


0.92 


0.0 


0.08 


HI 


0.00 


0.97 


0.03 


H2 


0.13 


0.03 


0.84 



7.3.1 Recognition of High Frequency Models 



As both a preliminary experiment and a useful tool, we examined recognition of high 
frequency events. The hrst experiment allowed transition between any pair of models. 
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Figure 7.9: Scatter plot of the feature vectors for the three stationary 
high frequency models. The contours show the 1 and 2 sigma bound- 
aries. 
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The context provided by the applied action and geometry was not provided. The 
second experiment used the applied action and geometry to constrain the allowed 
transitions between models. Since recognition is both segmentation and labeling, 
performance is reported in terms of the number of measurements that are misslabeled. 
Misslabeling can occur because transitions were missed or detected late, or a segment 
was just misslabeled. Because there are two sources of error, expected performance 
is lower than the estimated labeling performance. 

The labeling program uses the path-based formulation discussed in chapter 5 to 
estimate the current model. After estimating the two LPC coefficients and driving 
noise variance for each segment, the mean and square-root of the covariance of LPC 
coefficients was stored for each model. The mean of the variance was also stored 
for each model. These terms were then used in the orthogonal square-root Kalman 
filter to generate the test residuals. The change detector used a change magnitude of 
2.0 and a decision threshold of 8.0. A change of magnitude 2.0 means one standard 
deviation changes will cause the detector to accumulate the change statistic. A 
threshold of 8.0 produced an empirically good trade-off between false alarms and 
missed detections. 

The program charts an estimate of the probability of each model, given an input 
feature graph and models. It also displays the raw data and statistics about the 
current paths and the change history. The user interface is shown in figure 7.10. The 
program can be run both off-line and in real-time (on a 68040) using 250 Hz data 
generated by the signal processing. 



7.3.2 Context Free Recognition 



The hrst experiment allowed transitions between any two models. In addition, the 
initial probability of each model was equal. Under these conditions the training data 
had the following point level confusions. 







Ch 


osen Hypothesis 






HO 


HI 


H2 


H3 




HO 


6175 


119 


1508 





Correct 


HI 


19 


2859 


159 





Hypothesis 


H2 


654 


621 


4279 


208 




H3 





7 


12 


981 
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Figure 7.10: Display of the recognition program for high frequency 
events. The upper left figure shows the measured data. The upper 
right figure shows the feature graph. During execution, the current 
probability of each node is marked with color (not shown). The lower 
left shows the probability of each feature model as a function of time. 
The lower right shows the current models, their path log-likelihood and 
their change log-likelihood. 
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The systems marked 81% of all points correctly. Most of the errors are the expected 
confusions. The change detector increases the number of point confusions because 
models which do not quite match can take a long time to trigger a change. 

Most of the required transitions are marked very closely. The system does insert extra 
transitions. The number of marks with a given delay over the complete training set 
was: 



Delay 





1 


2 


3 


>3 




28 


8 


1 


3 


15 



Performance is lower with the test data. Over all the system marked 66% of the 
points correctly. The confusion matrix was: 



Correct 
Hypothesis 



Chosen Hypothesis 





HO 


HI 


H2 


H3 


HO 


4769 


86 


3302 





HI 


35 


2904 


222 





H2 


794 


1014 


2774 


101 


H3 





49 


12 


405 



For both the training and the test data, the major confusion is between contact and 
stationary. Since these two models are being separated purely on extra vibration 
sensed when the robot is touching an object, this is not surprising. Clearly sensing 
the wrench will eliminate this problem and greatly improve the scores. Moving is 
a sufficiently higher level of vibration that it is never confused with the stationary 
vibration level. Higher vibration levels which might be confusing with the moving 
level will result when moving along a surface. However, these can be distinguished 
from free space motions by the contact force. 

The decision delays were similar to the training set: 



Delay 





1 


2 


3 


>3 




22 


1 


2 





14 



7-4: Conclusion 
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7.3.3 Recognition with Context 



The performance of the system on the expected task can be greatly improved by 
providing context from the executed action and the geometry. The initial context 
is provided by the initial probability distribution over the models. The allowed 
transitions in the network encoded context of the expected sequence of models given 
the actions and geometry. This constrains the models to which the system can 
transition. 

The drawback is that unexpected events will not be detected by the system. Depend- 
ing on the network, these events could even cause the system to permanently deviate 
from the correct model. This trade-off exists any time an additional constraint is 
placed on the problem. The additional constraints help the path search so the change 
threshold parameter can be reduced and more of the paths can be searched. This 
will help prevent the permanent deviation problem. Also as in similar problems, a 
global reset can be used to restart the process when the measurements are very far 
from the current model. 

The test set was used in a second experiment. In this case, each data set used the 
appropriately constrained network. In addition, the initial state was set correctly. 
This increased the performance to 95% correct marking of the points from 66%. 
Almost all the errors occured on the seventh test run when the impact was not 
detected and the system remained in the moving state instead of transition to the 
impact and then the contact state. The confusion matrix was: 



Correct 
Hypothesis 



Chosen Hypothesis 





HO 


HI 


H2 


H3 


HO 


7981 


176 








HI 


18 


3142 





1 


H2 





636 


3946 


101 


H3 





47 


2 


405 



7.4 Conclusion 



This chapter showed how the theoretical ideas in chapter 5 could be applied to 
recognizing contact state from temporal models of the force and strain signals. 
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Autoregressive models were experimentally shown to be a good approach for au- 
tomatically segmenting the signal into similar components. The technique used a 
simple local change detection test that is relatively simple to develop and apply. 
The decision boundaries were very close to where hand marked boundaries would 
be placed. In addition the boundaries remain relatively constant as the decision 
threshold is changed. 

The AR models were of only marginal use in labeling textures. The AR coefficients 
do not encode substantially more information than what is available in the vibration 
energy, because there is not a lot of spectral content in the strain signal produced by 
stroking textures. However, they are still essential for producing the white residuals 
necessary for signal segmentation. 

Because the high frequency information is so important in recognizing transient 
effects and textures, an experimental recognizer was developed using only the high 
frequency data. This recognizer was able to estimate the correct model, significantly 
better than chance, without context. With context, the system was quite accurate. 
The only substantial error was made when an impact was not detected leaving the 
system stuck in an earlier state. 

These simple experiments demonstrated the utility of the approach. Constraint 
recognition has to be incorporated before a more complete task observer can be 
constructed. Chapter 8 investigates this problem. Finally chapter 9 presents some 
complete experiments that use both constraints and high frequency features in the 
task observer. 



Constraint Estimation 



Chapter 8 



Constraint is fundamental to manipulation. A robot must command motions or 
forces which will not violate the constraints in order to avoid generating substantial, 
perhaps damaging, contact forces. From the representational and estimation view- 
point of this thesis, constraint is fundamental because the majority of the measured 
wrenches are generated by quasi-static interactions with constraints. 

The held of robot force control has studied automatic compliance to constraints for 
many years. This large body of work has focused on two key problems: 1) repre- 
sentation of the constraints, and 2) writing control laws which automatically comply 
to the known constraints. Assuming the constraints are known can cause problems 
in grasping or more general manipulation. Often the geometry and pose of objects 
is only approximately known. For example, the geometry may have been extracted 
from vision. The resulting model could have substantial errors in the geometry. 
Grasped objects and parts that are to be mated often have large uncertainties in 
pose. 

The problem of estimating the constraints has received very little attention. A few 
papers have examined contour tracking in the plane when the geometry and location 
of the end-effector is known [Fedele et al., 1993]. A constraint estimation procedure 
where either the wrench of twist measurements was uses as a basis was formulated 
in [Bruyninckx and De Schutter, 1992], but we are unaware of any experimental 
results. 

This chapter solves the problem of estimating and identifying the constraints them- 
selves when both the geometry and the contact type is unknown for Cartesian spaces 
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with linear constraints and contacts between planar polygons. The estimator returns 
estimates of the geometric parameters which specify the contact constraints, and a 
set of measurement residuals. For multiple contact problems, uniqueness issues arise 
in recovering the contact geometry. 

The estimation problem is shown to result in a coupled generalized eigenvalue-like 
problem for which we give solution procedures. The resulting approach is robust 
to friction and random noise. For Cartesian spaces the approach can be modified 
and the effect of friction can be accounted for. Extending frictional modeling to 
rotational spaces is still a challenge. Extension of the approach to 6D0F is discussed 
in the conclusion. Finally the residuals from the estimator are used to determine 
the likelihood of different contact models. This was then applied to model-based 
labeling, segmentation, and recognition using the framework discussed in chapter 5. 

8.1 Constraint Estimation and Residuals 



Constraint estimation uses the measured wrenches and twists to best estimate the ge- 
ometric parameters of the constraint equation. Alternatively the measured wrenches 
and positions can be used to estimate the geometric parameters. For Cartesian 
spaces with linear constraints, both approaches result in the same equations. Since 
the position estimates are less noisy this is the preferred approach. In spaces with 
rotations, the extension is more difficult and has not yet been solved. 

This section provides an outline of constraint estimation and its relationship to con- 
straint labeling, segmentation, and recognition. The issue of changes in scale which 
preserve reciprocity is also discussed. The next sections present detailed estimator 
solutions for particular constraint cases. This chapter uses the discussion of config- 
uration space and constraints given in chapter 4. 

As discussed in chapter 4, configuration space constraint equations are of the form 

C(x,g) = (8.1) 

where x is the configuration of the robot and g is a vector of geometric parameters. 
The constraint wrench wc must lie in the cotangent bundle of the constraint mani- 
fold, and the velocity of the robot x must lie in the tangent bundle of the manifold. 
The fundamental property of constraints is that they cannot do work on an object. 
Therefore the reciprocal product of the wc and x must be zero. In Einstein sum 
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notation 1 

w Ct - x t - = 0. (8.2) 

The cotangent space is spanned by the partial derivative of C with respect to x, C x . 
Every constraint wrench is formed from a linear combination of the vectors in this 
basis. 

we; = C xik X k (8.3) 

The tangent space is spanned by C* and every velocity is formed from a linear 
combination of the vector in this basis. 

*•■ = Cx*, 7, (8-4) 

With these definitions the reciprocity condition 8.2 yields 

w Ci x,- = (C xik X k ) (C* xij 7j ) = (8.5) 

for every A and 7. Therefore 

Cx,-* C* Vj = (8.6) 

for every k and j and the bases are themselves reciprocal. 

Now suppose that a change of scale is applied to each wrench and velocity term. The 
change of scale is expressed as 

wc; = on w Ct x- = Pi x t - (8.7) 

The change of scale must preserve reciprocity. 

w' Ct i|. = (8.8) 

Expanding this condition and again using the fact that it must hold for all A and v 
yields the condition 

on (3, C xtk C* xtj = (8.9) 

for all k and j. This condition holds if and only if cti (3 i = d is a constant for every 
i. A change of scale of this form conserves the measured power. 

Given these preliminaries, consider the estimation problem. The hrst step is to 
change the scale of measured wrench and velocities so that the terms are unit-less. 
This can be done by dividing the measurements by the measurement standard devia- 
tions for each unit. Care must be taken in mixed units to obey the above reciprocity 



1 In Einstein sum notation, sums are assumed over repeated indices. 



142 Chapter 8: Constraint Estimation 



condition. In the new units, a unit-less tangent and cotangent basis can be used to 
produce the conditions 

c xij w ™* = ^wj (8.10) 

Cxi j x m , = Vxj (8.11) 

on the measured wrench and velocity. These two equations are the reciprocity con- 
ditions for the measured wrench and velocity. Under ideal conditions, the indicated 
reciprocal product would be zero. We make the statistical assumption that the prod- 
uct actually results in a normal, zero mean, random error. The covariance of these 
two errors depends upon the contact conditions. 

All of the statistical problems for constraint can now be stated. Estimation is the 
problem of computing, from a vector of measurements of the wrench and velocity, 
estimates of g, V|/ w , and V|/ . Segmentation is estimation followed by a change 
detection test on the residuals i/ w and v±. For labeling and recognition a hypothesis 
test is formed over the possible values of g, V|/ w , and V|/ given all the hypothe- 
ses. Labeling uses prior parameter values and a vector of measurements to form a 
likelihood hypothesis test. Recognition is simultaneous labeling and segmentation. 

The next two subsections discuss particular solutions to the estimation problem. 
The hrst subsection looks at the configuration space ?R. n with linear constraints. The 
solution in this space is relatively simple, and can be used both in its own right and 
as an input to a more general smoothing algorithm. The second subsection looks 
at the configuration space of planar polygons. This space is interesting because the 
configuration space constraints are curved. Despite this, it is still possible to develop 
an approach which uses all the data in estimating the constraints by using a curvature 
model of the constraint surfaces. 



8.1.1 Fast Solution for Linear Constraint in 5R A 



Estimation for a point in Cartesian space provides both a motivation for the more 
complex polygon solution, and a technique for doing local estimation followed by 
smoothing in a more general setting. We hrst consider contacts that have an associ- 
ated stiffness, and then consider the problem of rigid constraints. Since the problem 
is linear, the estimator can use the position measurements instead of the velocity 
measurements, which is more robust. Local smoothing would require the velocity 
measurements. 
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A free point in 3J has k free motions. Every contact with a hyper-plane in this 
space removes one of these free motions and introduces a relationship between the 
motions and wrenches. For a rigid constraint the relationship is that arbitrary forces 
against the constraint are possible and no motion is possible. Therefore, the rank of 
the allowed motions plus the allowed forces is always k. 

One way to write these constraints is to represent the contact as a stiffness relation- 
ship between the true configuration and wrench 

K(x-x )=w (8.12) 

where x is the zero point of the stiffness. However, this representation becomes 
poorly conditioned as the constraints become rigid, because elements of K go to 
infinity. The compliance representation will also break down when there is no con- 
straint. The difficulty is that one of the two coordinates x or w has been chosen 
as basic and the other one has been related to this basic coordinate through the 
constraint. The conditioning problem can be eliminated by directly representing the 
constraint in the joint measurement space. In this way both coordinates are treated 
equally. The joint measurement representation of the relationship is 



P T ( 



x 

w 



Po) = (8.13) 



where P T is a (A; X 2k) relationship projection matrix, and p is the zero point of 
the relationship. The stiffness form or the compliance form of the relationship can 
be recovered through column operations on P . 

The scaled measurements will not exactly obey equation 8.13, instead the projection 
of each measurement y t will leave a residual v t - 

P T (yt-Po) = ^ (8.14) 

The estimation problem is to determine the projection which minimizes the size of 
this residual over the measurement vector. The estimation problem can be formu- 
lated as a constrained maximum likelihood problem. 

The log-likelihood of the measurement residuals is 

1 n 

I = --(nlogdet \ u + Y,vJ\ t ,- 1 v t ) (8.15) 

z t=i 

with the constraint 

P T P = Id. (8.16) 
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Differentiating 8.15 with respect to V|/ and setting the result to zero yields 

1 n 
V„ = - 5>t"f. (8-17) 

n t=i 

After much further algebra, the log-likelihood can be shown to be 

— n 
l = —(\og&et{V u ) + k) (8.18) 

at this optimal value for V|/. Therefore, the optimality problem is to minimize 
logdet(Viy) subject to 8.16. 

Setting p to the mean of y t minimizes the criteria with respect to p independently 
of P. For a calibrated and zeroed force and velocity sensor the mean can be assumed 
to be zero. The mean can then be used to define an error variable y t = y t — p . The 
average covariance matrix X is the sum of the outer products of the error variables. 

From equation 8.14 and 8.15, the variance of the innovations is a projected version 
of J. 

Yjy = P T JP (8.20) 

It remains to determine the optimal value of P subject to the constraints. 

One solution is that P is the matrix formed by the k eigenvectors associated with the 
k smallest eigenvalues of X. Each of these vectors p 8 maps the values in 9£ 2fc onto a 
single dimension. This can be seen geometrically from the meaning of the eigenvalues 
of X. The eigenvectors form an orthogonal basis of 3J 2 aligned with the axes of the 
data. The projection operator projects the data onto a k dimensional subspace, and 
the determinant measures the volume of this subspace. The smallest volume will be 
produced when the projection takes the slice of the data that is aligned with the 
smallest directions in the data. 

The complete solution set consists of all projections which are solutions of the form 
PR that is rotations of P. This is proved by noting that 

det(P T JP) = det(R T P T JPR) 

since det(R) = 1. 
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8.1.1.1 Rigid Linear Constraints in $l k 



Rigid constraints introduce an additional reciprocity constraint on the log-likelihood 
criterion (equation 8.18). This constraint enforces the condition that the vectors in 
the tangent space (at a point) of a constraint must be reciprocal to the vectors in 
the cotangent space. Another view is that the estimator is seeking two vector bases 
P-r = {pj} and P^- = {p* k } at any configuration x which are reciprocal. These two 
bases can be collected into a single orthonormal, or rotation, matrix P = [Pr,Pr]- 
For Cartesian spaces, with linear constraints, all configurations on the same manifold 
surface have the same bases. The measurements of wrench must be reciprocal to the 
tangent basis (up to friction), and the measurement of change in position must be 
reciprocal to the cotangent basis. 



(8.21^ 



The constraint projection matrix P can be written in the form 

P r 
. P^- 

Therefore the log-likelihood criterion and the constraint on the projection matrix 
can be written as 

n a 

/ = max(log det V + k) subject to (8.22) 

V = P T JPand (8.23) 

P T P = Id (8.24) 

where the last equation is the reciprocity constraint. 

The reciprocity constraint makes minimizing this criterion difficult, and we had to 
solve it numerically. An alternative criterion based on the trace is easier to manip- 
ulate and produced good estimates. Unfortunately, the statistical meaning of the 
criterion is unclear. Therefore it was not clear how to use the criterion in recognition 
where different paths consisting of segments of different lengths must be compared. 
We discuss the trace procedure first, and then return to the original log-likelihood 
criterion. 

The original log-likelihood score is equal to the sum of the logarithms of the eigenval- 
ues of the error covariance. An alternative is to minimize the sum of the eigenvalues 
instead of their logarithms. Since the sum of the eigenvalues is equal to the trace of 
a matrix, this criterion is 



1 



2 T,pJ I iPi ( 8 - 25 ) 
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subject toP T P = Id (8.26) 

where p 8 is one vector in P and 1 8 - is its associated covariance matrix. 

Taking the derivative of equation 8.25, after adding in the constraints, with respect 
to each column of P results in the necessary conditions 

lip,- + P<r, = (8.27) 

where <r 8 - is the z th column of the constraint multipliers. Note that cr is a symmetric 
matrix. 

This coupled set of eigenvalue like problems can be solved to produce two bases which 
are reciprocal using Newton's method. A starting value was generated by solving 
an eigenvalue problem for each 1 8 -. This produces an initial guess for P which is not 
orthonormal. Newton's method then rapidly converges on orthonormal vectors. 

Two approaches to numerically solve the log-likelihood formulation of the problem 
numerically were explored. The hrst approach was to introduce the six constraints 
on the projection into the log-likelihood as a vector of Lagrange multipliers. Fifteen 
nonlinear equations result from the hrst derivative necessary conditions. The equa- 
tions can be simplified by introducing some auxiliary variables, increasing the number 
of equations and variables to twenty. The Hessian matrix for these twenty variables 
was then derived. An initial guess at the solution was again produced by solving an 
eigenvalue problem for each 1 8 -. The difficulty is that this initial guess produces a 
singular Hessian matrix. Although this was not a problem in the trace criterion (an 
addition of a regularizer solved it), it prevented the log-likelihood formulation from 
making any progress toward a solution. 

The next approach was to note that the solution sought is a rotation matrix. An 
initial guess at a rotation matrix was generated by orthogonalizing the matrix formed 
by the eigenvectors. The eigenvectors were sorted based on the magnitude of their 
associated eigenvalue from largest to smallest. Then the Gram-Schmidt orthonormal- 
ization procedure was applied to produce an orthogonal set. Then the eigenvectors 
were returned to their original order to produce an initial estimate Rmitiai- The 
numerical procedure then sought a rotation matrix R, represented by three Euler 
angles, which when applied to Rmitiai produce the optimal basis 

P = Rinitial R- (8.28) 

Powell's method of minimization [Press et al., 1992], which does not require deriva- 
tives, was applied because the symbolic derivatives were much too complex. Since the 
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initial estimate is usually close to the optimal estimate, Powell's method converges 
in a few iterations. 



8.1.1.2 Applying Constraint Estimation in 3J* 



Both stiffness-based and constraint-based estimation were applied to test data. The 
PHANToM with the fingertip sensor was moved by hand to produce four groups of 
four experimental runs. The hrst group is motion in free space, the second group 
is motion along a single flat plane made of wood, the third group is motion along a 
line formed by the plane of wood and an aluminum block, the last group is a fixed 
contact in a corner formed by the wood and two aluminum blocks. Sixteen hundred 
samples of the low-passed force and position data were sampled at 250 Hz. 2 

To investigate the effect of sample size on measurement accuracy, each group of 
four experimental runs was lumped into a single block of data. The measurement 
accuracy for the position and force measurements were then used to normalize the 
units of each measurement. The effects of friction where then removed from every 
sample using the following filter 

w,-= ( Id -^) w — ( 8 -29) 

This filter computes the power being expended by the motion, and normalizes it to 
get a frictional force. The velocity then gives the direction of the frictional force. 
This filter improves performance for the stiffness approach. 

Samples were then randomly drawn from the block of measurements to create an 
example for regression. Ten examples were drawn for each sample size. The stiffness 
regression was applied to the data and the estimated frame was returned in P. This 
was compared to an assumed true P by computing the singular value decomposi- 
tion of P T P = USV. If the two projection matrices were equivalent, the singular 
values would all be one. Since the singular values are the direction cosines of the 
misalignment, any deviation from one can be mapped to a positive angular error. 
We assumed a log-normal distribution for the deviation of the minimum singular 
value from f . Table 8.1 shows the mean and standard deviation (times yjn) of this 
log-normal distribution for each contact case, and the equivalent average angular 
error. Figure 8.1 graphically shows the effect of sample size on the estimated error. 



2 Chapter 7 describes the signal conditioning on the force signal 



148 



Chapter 8: Constraint Estimation 



Number of Constraints 





1 


2 


3 


Mean of Log-Normal // 


-10.9 


-5.09 


-4.00 


-10.0 


Std of Log-Normal ay/n 


1.3 


11.0 


23.2 


2.2 


Equivalent Mean Angular Err. (deg) 


0.34 


6.34 


11.0 


0.53 



Table 8.1: Error statistics for stiffness based estimation in 3J 3 usin^ 
position and force measurements and friction compensation. 



Angular Alignment Error and 1 Sigma Bounds (1 Contact) 



M 

•a 



o 

fcl 



3 

So 

a 
< 




Sample Size 

Figure 8.1: Angular alignment accuracy of the stiffness estimation pro- 
cedure with 1 contact as a function of the number of samples. Mea- 
surements of position and force compensated for friction were used in 
the estimation. 
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Number of Constraints 





1 


2 


3 


Mean of Log-Normal // 


-10.6 


-.228 


-1.78 


-10.0 


Std of Log-Normal ay/n 


1.54 


6.37 


5.89 


1.9 


Equivalent Mean Angular Err. (deg) 


0.40 


78.2 


33.7 


0.55 



Table 8.2: Error statistics for stiffness based estimation in 3J 3 usin£ 
position and force measurements without friction compensation. 



The data shows, as expected, that both the full contact and no contact case produce 
very accurate results with little deviation. The contact cases have some bias and 
both have significant variance. A plot of the position data shows that the wood 
surface was not flat. For some data sets an 11° slope can be observered. How much 
of this is kinematic error in the PHANToM and how much is actual alignment error 
would require more careful measurements. It is very possible that the kinematic 
chain from the block, to the table, to the PHANToM, and finally to the fingertip has 
1° — 2° of angular error. 

Secondly, the data from the experiment with two contacts is not perfectly straight. 
There is enough kinematic error in the robot to produce a 1 mm deviation from a 
straight-line fit to the x and y position data over a maximum of 80 mm. In addition, 
the significantly greater standard deviation in the measurements is probably due to 
friction and vibration. 

For comparison the same experiments were run without hrst preprocessing the data 
to account for friction. Table 8.2 shows the results. As the table indicates, stiff- 
ness based estimation without friction compensation is terrible. Friction completely 
contaminates the measurements and for one contact produced an estimate that is 
almost 90 degrees opposed to the correct normal. 



The constraint based estimator was also tested. The same sampling procedure was 
used to generate examples for the constraint based estimator. The results are signif- 
icantly improved by adding the information on the number of constraints. Table 8.3 
summarizes the results. Figure 8.2 shows the performance of this estimator as a 
function of sample size. Even the results without friction compensation, and esti- 
mation based on velocities are quite accurate. Tables 8.4 and 8.5 show the results 
for estimation without friction compensation, and for velocity based estimation with 
friction compensation. 
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Number of Constraints 





1 


2 


3 


Mean of Log-Normal // 


NA 


-8.5 


-10.2 


NA 


Std of Log-Normal 0\fn 


NA 


7.3 


0.38 


NA 


Equivalent Mean Angular Err. (deg) 


0.0 


1.2 


0.5 


0.0 



Table 8.3: Error statistics for constraint based estimation in 3J 3 usin£ 
position and force measurements with friction compensation. 



Number of Constraints 





1 


2 


3 


Mean of Log-Normal // 


NA 


-8.19 


-10.2 


NA 


Std of Log-Normal 0\fn 


NA 


7.2 


0.36 


NA 


Equivalent Mean Angular Err. (deg) 


0.0 


1.34 


0.5 


0.0 



Table 8.4: Error statistics for constraint based estimation in 3J 3 usin£ 
position and force measurements without friction compensation. 



Number of Constraints 





1 


2 


3 


Mean of Log-Normal // 


NA 


-10.5 


-9.9 


NA 


Std of Log-Normal ay/n 


NA 


10.6 


8.7 


NA 


Equivalent Mean Angular Err. (deg) 


0.0 


0.43 


0.59 


0.0 



Table 8.5: Error statistics for constraint based estimation in 3J 3 usin£ 
velocity and force measurements with friction compensation. 
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Angular Alignment Error and 1 Sigma Bounds (1 Contact) 




Sample Size 

Figure 8.2: Angular alignment accuracy of the constraint estimation 
procedure with 1 contact as a function of the number of samples. Mea- 
surements where positions and forces compensated for friction. Note 
the significant improvement in performance over the stiffness approach. 



The completely free and fixed case are fit without any error (indicated by NA), since 
there is nothing for the estimator to estimate. The partial constraint cases are also 
significantly improved. 

If accurate estimates are required for assembly or other mating tasks, it may be 
worth the additional computational cost to use the constraint based approach. The 
eigenvalue computations are easier in the constraint approach since two k X k sys- 
tems have to be solved, versus one 2k X 2k system. However there is additional 
computational cost in the numerical iterations. In the test cases, this computation 
generally converged in two to three steps from the initial estimate, but each step 
requires searching for a minimum in three different directions. 



8.1.2 Estimation for Planar Polygons 



The configuration space for planar polygons is C = 3J 2 X S0{2). Each configuration 
can be represented by a homogeneous transform T. The rotation component R of 
the transform can be identified with a single angle 6, therefore the configuration 
space coordinate x is equal to the triple (r X} r y} 6). The position component (r X} r y ) 
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will be represented by the vector r. With these conventions, the planar rotation is 
given by 



R(0) 



c — s 
s c 



(8.301 



cos(#) — sin(#)" 
. sin(#) cos(#) 

where c and s are abbreviations for cos and sin. This representation for the configu- 
ration space will make some of the derivations easier. 



For polygons any contact constraint consists of contacts between one or more points 
and lines. Let p be the homogeneous coordinates of a point in space. Let n = [ n, —d] 
be the homogeneous coordinates of a hyper-plane (line in 2D) with n a unit vector, 
and d the distance to the plane measured along the normal. With this notation the 
basic constraint that a point lie on the plane is 

°n To p = 0. (8.31) 



This general constraint becomes a function of the configuration of the polygon. There 
are only two basic types of contact that can occur with polygons. Type B contact is 
contact between a vertex on the moving polygon and an edge on the fixed obstacles. 
Type A contact is contact between an edge on the moving polygon and a vertex on 
the fixed obstacles. Type B constraints are most easily expressed in the fixed frame, 
and by symmetry type A constraints are most easily expressed in the moving frame. 
However, for uniformity, all the constraints will be given in the fixed frame. 

For type B contacts the general constraint becomes 

°n TO T m m p = 0. (8.32) 

In this equation m p is the coordinates of the contact vertex expressed relative to 
the moving frame, °T m is the transform taking the local coordinates to the global 
coordinates, and °n is in the fixed frame. This equation can be expanded to 

°n T (°R m p + r) - d = 0. (8.33) 



For type A contact the constraint is 

m n T °T" 1 °p = 0. (8.34) 

In this equation °p is the coordinates of the contact vertex expressed relative to the 
fixed frame, m n is in the moving frame, and °T~ 1 is the transform taking global 
coordinates to local coordinates. It is the inverse of °T m . This equation expands to 

m n T0 R^ (p-r)- d = 0. (8.35) 
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The difference between the two constraint equations is whether °R m is applied or 
not applied to r. 

Both constraint equations involve a normal. To simplify the following notation we 
will drop the frame superscript. The choice of frame should be clear from the contact 
type. 



8.1.2.1 Constraint Tangents and Cotangents 



This section derives the constraint tangents and cotangents for both type A and type 
B contacts. It is shown that all the tangent bundles can be written as a projection of 
the product of a 4x4 curvature matrix with a constant 4 vector. This unifying view 
of the problem allows us to derive a single general form for the constraint estimator 
in the next section. In all of these equations R = °R m to simplify the notation. 



8.1.2.2 Type B 



Taking the derivative of equation 8.33 with respect to x gives the cotangent vector 



Y B (x) 



n 



n T XRp 



(8.36) 



where X 



-1 

1 



and is the cross product matrix. X has the properties 



• y T Xy = for any vector y 
. X T = -X 

• and XR = RX, i.e. X commutes with rotations. 



Expanding out the term n T XRp results in the expression 

n T XRp = n T Xpcos(#)-n T psin(#) 
= [1 ] Ra, 



(8.37) 
(8.38) 
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where a\ = n Xp and a 2 = n p. This means that Y#(x) can be rewritten as 



Y R fx) 



Id 
R 



P Cy B Y B , 



(8.39) 



where Id is the identity matrix and P is a projection matrix which keeps the hrst 
three components. Y# will refer to the constant four vector and Y#(x) will refer to 
the function. The matrix Cy B encodes the curvature of the cotangent vectors and 
plays a critical role in the estimation problem. 



The tangent vectors J Bl and J^ 2 are reciprocal to Y B 

RXV 



J Bi(x) 



Xn 





Jb 2 (x) 



1 



(8.40) 



and these can also be written as products of the projection matrix and curvature 
matrices as 



Jb, (x) 



J B 2 (x) 



rid 


1 


rxni 


. 


Id. 





[ R 


1 


rx^p 
i 




.0 


Id J 



P c Jm J Bl 



P Cj„ 2 Jb 2 . 



(8.4i: 



(8.42) 



8.1.2.3 Type A 



Taking the derivative of equation 8.35 with respect to x gives the cotangent vector 

Y -< X >=[nW( n p- r) ] < 8 ' 43 » 

Expanding out the term n T X T R T p results in the expression 

n T X T R T p = n T X T p cos(#) - n T p sin(0). (8.44) 

Using the previous definitions of a^ and a 2 shows that Y^(x) can be rewritten as 



Y 4 (x) 



r r 


n 1 


r _. 




rr T XRl 






—ii 







R 




a 



p c Ya y a . 



The tangent vectors J Al and J A2 are orthogonal to Y^ 

RX T n 





J ^(x) 



j a 2 (x) 



X(p - r) 
f 



(8.45) 



(8.46) 
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and these can also be written as products of the projection matrix and curvature 
matrices as 



«U (x) 



j a 2 (x) 



R 
Id 







Id [Xr 0' 
Id 



P Cj,. J^ 



Xp 

1 





P Cj, 2 J A2 . 



(8.47) 



(8.48) 



8.1.3 General Case 



The dimension of the tangent and cotangent vector space, at a point, is always 
three for planar rigid bodies. That implies that for a single constraint the cotangent 
vector can be computed from the cross product of the two tangent vectors up to 
sign. With two constraints there are two cotangent vectors and only one tangent 
vector. Therefore, the tangent vector can be computed as the cross product of the 
two cotangent vectors. 

Therefore, we can always choose two vectors (Ui,U2) as the base vectors for the 
appropriate bundle. Let Ci and C 2 be their respective curvature matrices. Then 
the third vector \(x) can be generated at any configuration by 

V(x) = Ui(x) x U 2 (x) = C(C 1 (x), C 2 (x)) Ui ® U 2 (8.49) 

where Cg) is the tensor product of Ui and U2 arranged into an appropriate vector, 
and C is the appropriate rearrangement of the two curvature matrices to give the 
required result. For any definition of arrangement of Cg) and any 3 X n matrices X 

and Y 

~X 2 <g) Y 3 - X 3 <g) Y 2 ' 

C(X,Y)= X 3 ® Yx - Xx ® Y 3 
_Xi ® Y 2 - X 2 ® Yi _ 

where X 8 - denotes the z th row of X. As a further notational convenience let Ui (S0U2 = 
T e (U2) Ui = T e '(Ui) U2, where T e and T' e are appropriate arrangements of the terms. 
The rest of this development will use these general definitions. 



8.1.3.1 Equivalent Systems of Two Contacts 



Before presenting the solution for the contact estimation problem, this section dis- 
cusses the question of uniqueness. For two or more contacts the geometric parameters 
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which specify the constraint manifold are no longer unique. There are three separate 
cases: 1) two contacts along a line, 2) two same type contacts not in a line, and 3) 
two different type contacts not in a line. 

The solutions are not unique because for any multiple contact situation there exists 
an infinite set of mechanisms which produce the same configuration space curve. 
This is very clear for contact along a line. Contact along a line creates a straight 
line constraint in C. The same constraint is created by any pair of collinear sliders, 
with the same angle, attached anywhere on the body. Therefore, all such sliders are 
equivalent . 

The problem of equivalent mechanisms is not a special problem of line-line contacts. 
Figure 8.3 shows two type B contacts on a body, the associated configuration space, 
and one equivalent mechanism. The motion of x is a straight line in Cartesian space 
and a helix in C. Kinematically, the contact set acts as a pair of pivots attached 
to two fixed sliders. For any choice of direction for one of the sliders there exists a 
unique second direction and two attachment points on the body which will produce 
exactly the same configuration space surface. 

To prove this, let Y^ = \n\ n 2 a\ a 2 ] and Y^ = \m\ m 2 b\ b 2 ] be the two 
bases for the two contact wrenches. Computing the twist freedom J from the cross 
product J(x) = Y n (x) X Y m (x) yields 



J (B,B)( X ) 



(n 2 h — m 2 a 1 ) (m 2 a 2 — n 2 b 2 ) 







c 




(m\a,\ — n\b\) (riib 2 — niia 2 ) 







s 


(8.50) 





(n 1 m 2 — n 2 m x ) _ 




_1_ 




J T(B,B)f(B,_B)(x). 








(8.51) 



Therefore equivalence of two mechanisms is therefore the same as equivalence of the 
induced matrix Jt(b,b)- We term this matrix the tangent space tableau. 

For any n, Jt(.b,.b)(3, 3) = z 5 determines the value of a corresponding m. Then an 
equivalent matrix can be created by solving the equation 



-m 2 





n 2 


" 




a,\ 




J T(B,B)(1,1 





m 2 





-n 2 




a 2 




Jt(b,b)(1 5 2 


rrii 





-n-i 







W 




Jt(b,b)(2, f 





— mi 





ni . 




[b 2 \ 




.Jt(b,b)(2,2 



(8.521 



) 2 . Therefore, the solution is 



The determinant of this matrix is (niin 2 — m 2 ni) 2 = (z 5 

unique as long as m / n. That is the constraint is not line-line contact. This shows 
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Configuration Space Constraint Curve 



Transformation to 
Configuration Space 



Kinematically 
Equivalent to 





Transformation to 
Configuration Space 



2 independent 
slider, rotary pairs 



Figure 8.3: The configuration space for two non-collinear type B con- 
tacts and an equivalent mechanism. The triangular part is contacting 
two walls. Motions that obey these two constraints cause the control 
frame x to move in a spiral in configuration space. The same spiral can 
be produced with the indicated mechanism. 
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that type (B,B) contact is unique only up to the initial choice of one of the contact 
normals. Furthermore, it shows that for linear constraint the contact locations are 
arbitrary. 



A similar result holds for the other contact pairings. For type (A, A) the tangent 
space tableau and vector are 

(a T Xm-b T Xn) 



(-a T Am+b T An) (a T Zm-b T Zn) 
2 2 

(a T Zm-b T Zn) (-a T Am+b T An) 
2 2 





-m T Xn 

m T Xn 





(a T m-b T n) 

2 

m T Xn 



J ( a,a)(x) = Jt(a,a) [ cos ( 2 ^) sin(20) r x r 2 1 ] J 



where Z 



"-1 0" 


, A- 


'0 1" 


[ lj 




[i oJ 



(8.53) 



(8.54) 



and (a, b,n,m) are the appropriate 2 vectors. 



Finally, for type (A, B) the result is 



'T(i,B) 



*(A,B) 



X 



-(a T Am) -(a T Zm) 



(a T Zm) 



-(a T Am) 



2 




2 


-ni2»2 




m 2 ra 1 


m 1 ra 2 




— mini 


-m 1 ra 2 




mini 


m — 2ri2 




m 2 ni 


b\n 2 




-bini 


-b 2 n 2 
(a T Xm 

2 


b 2 n 1 

(a T m) 

2 

" cos(2^) " 

sin(2^) 

r\ cos(6) 

r 2 cos(6) 


V(A,B) 


r 2 


sm(9) 
sm(9) 
cos 
sin 
1 










m T Xn 
m T n 




(8.55) 



(8.56) 



8.1.3.2 The Planar Constraint Estimator 



All of the planar constraint problems can be stated using the appropriate basis vector 
pair (Ui,U2) and their associated curvature matrices. The third vector V can be 
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formed from the cross-product of these two vectors. We consider only the trace 
criterion 8.25 for the planar polygon problem. In this problem, we have to define 
three matrices: 

I, = ->:C(x,;yu(i)u(irC(x,;) (8.57) 



I 



Vl 



EC(x,) T u(i)u(i) T C(x,.) 

i 

^CMx.-fv^vtifC^x,- 



1 V2 = -£C 2 (x 8 ) T v(i)v(i) T C 2 (x 8 ). 
n 



(8.58) 



(8.59) 



The first matrix can be used to compute the projection of the measurements onto 
V, but V is being determined by the cross-product of Ui and U2. The second two 
matrices can be used to compute the projections of the v 8 - measurements onto Ui 
and U2. Using these definitions the criterion to be maximized can be written as 



/ = V T J„V + U/LUi + U 2 T J„ 2 U 2 



(8.60) 



Finally, define 



and 





rid 


01 






. 


0. 




























1 















The constraints can now be added to create the final optimization problem 



r 



V J J„V + Ui J J^Ui + u 2 J x„ 2 u 2 



(8.6i: 



+ i ??1 (U 1 T C 1 U 1 - I) + ^ 2 (U 2 T C 2 U 2 - I) - 7 fD 1 Ui - ^B 2 V 2 . 



The Cj- are one of the two constraint matrices I or I depending upon the constraint 
type. The D 8 are additional constraint conditions that arise in the single contact case 
because in this case Ui and U 2 have constrained forms, and 'y i are their associated 
Lagrange multipliers. 



Differentiating and using the different expressions for ® yields the two necessary 
conditions 



T e (\J 2 y l u T e {U 2 )U 1 +l Vl Ui+D^j = j/iCxUi 

r e '(u 1 ) T j u r e '(u 1 )u 2 +j„ 2 u 2 + D 2 r 7 2 = ^c 2 u 2 . 



(8.62) 
(8.63) 



The result is two coupled generalized eigenvalue like problems which must be solved 
simultaneously. The solution procedure depends upon the particular contact type. 
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8.1.3.3 One Contact 



In the one type B contact case Ui = J_b 17 U2 = J_b 2 , C\ = 1 and C'2 = I. The 
solution procedure consists of iteratively solving a single eigenvalue problem and a 
linear equation. Let 



X = T e (U 2 ) T X„T e (U 2 )+X 



Vl 



11 



X 

x 12 



Xl2 
X 2 2 



(8.64) 



Then the eigenvalue problem Xnri = 7/1 ri results from equation 8.62 where X\\ is the 
indicated 2x2 submatrix. After solving this problem, let 



j = r e '(u 1 ) T j„r e '(u 1 ) + j„ 



Xn 


2l2 


Xl3 


X T 

x 12 


X 2 2 


X23 


-^13 


-^23 


X23 



(8.65) 



Then the linear equation XqD = X i2 results from equation 8.63 where X\\ is the 
indicated 2x2 matrix and X i2 is the indicated 2x1 matrix. The solution for ri 
and p can be determined by iterating these two solution procedures. Because the 
cost metric is strictly positive definite the solution will be unique. Furthermore, the 
procedure has a quadratic convergence rate. 



After solving for ri and p the final solution is given by 

ri = Xn p = X T p. 



(8.66) 



One type A contact results in the same equation for p and ri, however the final 
solution is given by 



ri = X T n 



XV 



(8.67) 



8.1.3.4 Solution Procedure for Two Contacts 



Constraint equivalence appears in the estimation solution as an indeterminacy in the 
basic equations (8.62,8.63). Expansion of 

T e (U 2 ) T I u T e (U 2 )+I Vl 

for the line-line contact case shows that for every U2 the resulting value is only rank 
1. The minimum is obtained by selecting the eigenvector which has nonzero normal 
component and the smallest eigenvalue, and the eigenvector which has a nonzero 
value in the third component. 
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For two contacts of the same type the two estimation equations are redundant. There- 
fore two eigenvectors are sought for one of the equations. For the non-collinear case, 
the two solution eigenvectors are the two vectors with nonzero normal component. 

Finally, with two contacts of different type both equations must be simultaneously 
satisfied. High convergence rates are obtained by iterating between the two equations 
using the solution for the smallest eigenvalue at every step. 



8.1.3.5 Simulated Estimator Performance 



To confirm that the estimator algorithm works correctly, the wrenches and motions 
from different contacts were simulated. The system generated an example desired 
trajectory of fOO points using a random motion bounded to stay within a (-0.3, 
0.3) radian range. The translational random walk was always of unit length f . A 
constraint solver is then used to produce the output motion by minimizing the energy 
in a stiffness matrix subject to the constraints. The constraint forces and the motion 
are recorded and then read back into the estimator simulation. 

The true force and velocity measurements were then corrupted using a normal noise 
model. The average variances of the translational and rotational velocities, and of 
the force and torques are computed. The noise magnitude is then set to the product 
of the average variance with a signal-to-noise ratio. For point contacts (type A and 
B) the torque error was set to zero, because no torque can be applied about the 
point contact. For each configuration, a basis which completes the tangent space 
and a basis which completes the cotangent space is chosen. For type B contacts the 
velocity noise basis is the normal to the contact line and the force noise basis is the 
direction along the contact line. A normal noise was then generated, multiplied by 
the appropriate basis vector, and added to the true value to get the measurement 
value. Because the algorithm relies on the relative magnitude of the forces and not 
the absolute values, noise does not have to be added in the direction of the constraint 
to test the system. 

The estimator for the appropriate model was then run on the simulated data. Lengths 
of (f , 2, 8, f 6, 32, 64) and signal-to-noise ratios of (0.707, O.f , O.Of , O.OOf , O.OOOf ) were 
used. Figure 8.4 shows the average simulated accuracy of the estimator for a type 
B contact. The results for type A experiments were similar. For this type of noise 
the angular error is unbiased and the variance decreases as the signal-to-noise ratio 
increases. The errors depend on the product of the length and the signal-to-noise 
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Figure 8.4: Angular alignment accuracy and relative position error for 
the polygonal constraint estimation procedure for a type B contact as 
a function of the product of the contact vector length times the signal- 
to-noise-ratio. 



ratio. A better signal-to-noise ratio or a longer length decreases both the relative 
position error and the angular error. For products on the order of 100 both estimates 
are quite accurate. A value between 10 and 100 can be reasonable expected. The 
simulation suggests that the technique should work well in practice. 



A type BB contact test was also performed. In this test, two perpendicular walls 
were used as the constraints. The walls were equally distant from the origin with 
distances of (1, 2, 8, 16, 32, 64). The same signal-to-ratios were investigated. Figure 
8.5 shows the average simulated accuracy of the estimator for a type BB contact. 
The position accuracy was computed from the contact tableau for this contact type. 
The hrst two elements of each of the hrst two rows was used as a position vector and 
this was compared to the true tableau. The angular error was computed by finding 
the maximum angular error between the direction cosines of these vectors. Note 
that this measure of angular error is guaranteed to produce positive errors. As the 
figure shows, type BB contacts are more difficult to estimate. It takes a product of 
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Figure 8.5: Angular alignment accuracy and relative position error for 
the polygonal constraint estimation procedure for a type BB contact as 
a function of the product of the signal-to-ratio and the contact vector 
length. 



length and signal-to-noise ratio of 1000 before accurate measurements of geometry 
are produced. 



8.1.4 Constraint Extensions 



This section looked at two particular constraint estimation problems. Algorithms 
based on minimizing the measurement error covariance were derived and experimen- 
tally tested. Both algorithms produced good estimates of the constraint directions 
and the geometric parameters. However, the derivations are limited in generality. 
There are two extensions that would be useful for general manipulation: 1) Exten- 
sions to polyhedral interactions, and 2) general stiffness/constraint representations. 
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The cross-product approach used in the planar derivation will not work for polyhe- 
dral interactions. In addition, the representation of configuration breaks down. The 
hrst problem can be potential solved by treating both the tangent and cotangent 
spaces equally and then enforcing the reciprocity condition through a Lagrange mul- 
tiplier. The second problem might be solved by changing to a representation based 
on the dual-quaternion [McCarthy, 1990]. This approach was used for linear con- 
straints with the trace criterion. Since quaterions represent the rotation implicitly, 
the problem of representational singularity that occurs in an angle based representa- 
tion does not occur. Furthermore, McCarthy's exposition shows how the constraints 
of kinematic mechanisms can be uniformly represented in terms of vector quadratic 
equations. This representation might simplify the derivation of results equivalent to 
the polygonal contact results. Constraint representation based on quaternions is also 
used in [Canny, 1987]. 

A more general representation is needed for bodies with complex geometries or com- 
pliant surfaces. The stiffness estimation procedure combined with a smoothing algo- 
rithm may provide such an approach. Stiffness estimates could be generated locally 
from change in position and wrench measurements for a whole range of locations and 
operating conditions. This second order tensor held could then be smoothed using a 
regularizer based smoothing technique. This might provide a general representation 
for what is felt, at low frequencies, when two objects interact. One speculation is 
that this could be used for representing the additional constraint/stiffness that is felt 
as two objects engage in an assembly. A recursive state estimator could then use 
this held and the wrench and position measurements to estimate the configuration 
of an interaction. 



8.2 Determining the Degree of Constraint 



There are two basic questions we can ask in constraint labeling: I) can the degree 
or form of the constraint be recognized from the measurements, and 2) to what 
degree can constraints with different geometric parameters within the same form be 
differentiated. This section looks at the hrst question, the next section formulates 
and tests the statistics for the second question. 

The constraint based estimator can be used directly to determine the degree of 
constraint because it breaks all possible projections down into constraint classes. 
The class can be determined by comparing the projected trace log-likelihood scores 
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for the best projection, returned by the estimator, for each class. 

To test the approach four hundred blocks of 25 points, each of which takes 0.1 seconds 
to measure, were pulled at random from the measured data for each constraint class. 
The estimator was then run on each sample block, and the model with the highest 
log-likelihood score was selected as the correct model. The approach yielded the 
following confusion matrix: 

Chosen Hypothesis 
Number of Constraints 










1 


2 


3 







1.00 


0.00 


0.00 


0.00 


Correct 


1 


0.00 


0.97 


0.03 


0.00 


Hypothesis 


2 


0.00 


0.00 


0.97 


0.03 




3 


0.00 


0.00 


0.00 


1.00 



Another approach to determining the number of constraints is to use the eigenvectors 
from the stiffness regression. The degree to which these eigenvectors align with the 
ideal motion or wrench projection directions, computed from the singular values, 
can be used as a measure of constraint. The test statistic is the sum of the sample 
alignment probability statistic for each selected triple of vectors weighted by the 
probability that the selected triple is the correct triple. This approach has the 
advantage that it uses the relatively fast computation of the eigenvectors for the 
comparison, followed by many 3x3 singular value computation. 

One alignment statistic is the sum of the singular values of the upper 3x3 matrix 
from P, and the sum of the singular value for the lower 3x3 matrix from P. The 
singular values are the direction cosines between the two bases. The mean and 
covariance of this alignment statistic can be computed from the contact examples. 
The test is then 

p(x?|W) = X;p(s(P(i))|W)P(i) (8-68) 

i 

where i is a three element index, s is the singular values computed from the columns 
selected with the index, and P(i) is the probability that the given index contains the 
eigenvectors associated with the smallest eigenvalues. 



The index probability, and the associated summation, is what makes this approach 
difficult. For 3 eigenvectors of size 6 there are =20 possible indices. The index 



probability can be computed from the asymptotic normal statistics of the eigenvalues 
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given in [Anderson, 1984]. The index probability is the probability that the selected 
eigenvalues fall in the given order given the uncertainty in their estimates. If the 
data is sufficiently rich such that only one of these probabilities is relevant then the 
test is very fast. However if the data is just noise, which may have been measured 
prior to exploration, all of the terms will be necessary. 

A simplified version of the test was tested by generating four hundred blocks of 
25 points as in the constraint test. These blocks were assumed to be sufficiently 
rich, so that only the three eigenvectors associated with the three smallest estimated 
eigenvalue had to be compared. Because the data was rich, this gave very good 
results. 

Chosen Hypothesis 
Number of Constraints 










1 


2 


3 







1.00 


0.00 


0.00 


0.00 


Correct 


1 


0.00 


1.00 


0.00 


0.00 


Hypothesis 


2 


0.00 


0.005 


0.995 


0.00 




3 


0.00 


0.00 


0.00 


1.00 



However, when the same approach was applied to noise, the test selects one of the 
hypotheses at random but assigns a very high relative likelihood to its guess. This 
is wrong, and can only be fixed by including the summation over all indices in the 

test. 

Given this additional complexity, and uniformity of the maximum likelihood frame- 
work for the constraint based approach, I feel that the constraint approach is a better 
test. 



8.3 Constraint Direction Labeling 



The recognition power of constraints is significantly enhanced by testing the direc- 
tion of the constraints. This section adds two additional log-likelihood terms onto 
the constraint degree-of-freedom penalty to account for prior information about the 
constraint frame. With this enhanced model contacts with the same number of con- 
straints but different constraint frames can be distinguished. This is demonstrated 
with experiments. 
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Recall that the log-likelihood criterion, given the measurements of position and force, 
for the number of constraints is 

n a 

/ = max(log det V + k) subject to 

V = P T JP and 

P T P = Id(equations 8.22 to 8.24). 

where 1 = \ J27=i ftfj (equation 8.19). 

Prior information on the nominal constraint frame can be added to this criterion 
by adding a penalty for the deviation in alignment of the estimated frame from 
a nominal frame. The alignment deviation can be measured, as in the estimation 
results section, by the singular values of the product of the estimated projection 
matrix and the nominal projection matrix 

3 

a = 3 - J2 svd(P T Pjv) t - (8.69) 

8 = 1 

where P is the estimated 6x3 projection matrix, Pjv is the nominal projection 
matrix, and svd 8 - is the z th singular value. The sum is bounded between and 3 and 
measures the alignment of the frames. 

An exponential distribution provides a convenient prior distribution for a. The expo- 
nential distribution is defined over positive numbers, is maximal at 0, and decreases 
exponentially. With this prior distribution, the combined log-likelihood criterion is 

n a 

/ = max(log det V + k) + log(a) — act subject to (8.70) 

V = P T JPand (8.71) 

P T P = Id. (8.72) 

where a is the parameter of the exponential distribution. A convenient transforma- 
tion from the prior standard deviation of the expected angular alignment error to 
the parameter of the exponential distribution is 

a = 2/3(1/1 -cos(A0)). 



This additional criterion makes it possible to distinguish contact frames that are 
quite closely aligned. Figure 8.6 shows the experimentally measured probability of 
error as a function of the angular difference between two candidate frames. The true 
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Figure 8.6: Experimentally measured probability of error as a function 
of the angular difference between two frame hypotheses. There is more 
error with two contacts than one contact. Over 98 % correct selection 
is demonstrated for differences in frame direction of 20 degrees. 



frame was rotated by the indicated amounts about the x axis to create the alternative 
frame. Fifteen degrees was taken as the expected standard deviation of the alignment 
error for both frames. Twenty-five measurements were used for all decisions. One 
hundred samples were used in the estimation procedure for both models. The figure 
shows that even with only a 5 degree difference in the frames, the system is still able 
to select the correct frame over 90 % of the time. 

This approach has one problem, in that it is unable to distinguish between two frames 
that span the same vector space, but have opposite signs in the cotangent basis. That 
is, the system cannot distinguish between contacts on the top of a plane and on the 
bottom of the plane. This problem is a consequence of the decision, discussed in 
chapter 4 to consider contacts as bidirectional constraints. Clearly these two types 
of contacts are easy to distinguish, and in fact can be distinguished purely from the 
applied action. However, fixing this problem within the feature framework requires 
some careful thought. 



The hrst approach, that might be considered, is to simply test if the measured force 
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lies in the positive span of the hypothesized force basis. But this is just the fixed 
angle friction cone model for forces, and as was already pointed out this model is 
not a feature in spaces with curvature. 

A possibly better approach is to explicitly account for the unidirectionality of the 
constraint bases in the log-likelihood criterion. It may be possible to rework the 
estimation procedure in terms of a basis representation consisting of a collection 
of unidirectional basis vectors for the tangent and cotangent spaces. Each contact 
would shift one of the unidirectional vectors from the tangent space into the cotangent 
space. In this way the log-likelihood criterion would be able to distinguish between 
different contact signs. The singular value criterion could be similarly modified to 
measure the alignment of the unidirectional bases. 



8.4 Conclusion 



This chapter discussed the basic problem of constraint estimation and labeling. Con- 
straints and changes in constraint are fundamental properties of manipulation. The 
ability to estimate the constraint directions and detect deviation from the current 
estimate are an essential component of any contact observer. 

This chapter presented a log-likelihood based approach to constraint estimation. 
By considering projections of the forces onto the tangent bundle and velocities (or 
deviations in position) onto the cotangent bundle as normal noise, we were able to 
derive a log-likelihood criterion. This criterion was scale independent and considered 
sequences of measurements. Furthermore, since the criterion worked with the joint 
measurement information in the velocities and forces, the criterion was independent 
of the number of constraints. 

The same basic approach was used for both Cartesian spaces and polygonal object 
spaces. The major difference between the two was in the form of the equations 
specifying the cotangent and tangent bundles. For Cartesian spaces, the tangent 
and cotangent bundles are constant as a function of position. However, in polygonal 
object spaces, the tangent space depends on the configuration and the notion of 
multidimensional curvature must be introduced to the estimation. For polygonal 
objects, the curvature depends only on the configuration and not on the basis vectors 
specifying the tangent and cotangent bundles. The problem could thus be separated, 
and again all of the information in a sequence of measurements could be exactly 
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combined to estimate the basis of the bundles. 

This basis is not unique in both the Cartesian space problem and the polygonal 
object contact problem. In Cartesian space, the basis in unique only up to a rotation 
that preserves the space spanned by the basis. In the polygonal space the problem 
is similar. Two bases were shown to be equivalent if they produced the same contact 
tableau. 

Both estimators were implemented and tested. The Cartesian estimator was tested 
on real data, and the polygonal estimator on simulated data. In both cases, the esti- 
mators were extremely robust to noise, and the Cartesian estimator was able to reject 
the effect of friction. This shows the power of using a sequence of measurements. 
With multiple measurements and a little exploration, the frictional force tends to 
average out. 

The Cartesian estimator was shown to lead directly to a log-likelihood procedure for 
labeling the number of degrees-of-freedom in a contact situation. In addition, with 
a small modification it was also able to determine the most likely direction of the 
constraints given two candidate models. 

The procedures work because a little exploration gives a rich amount of data on which 
to base a decision. The main issue in performance, and any estimation problem, is 
sufficient richness of the data. Taking many measurements at one location or force 
level really does not improve performance. The important thing is to move about 
locally or apply forces in a variety directions. Then the model-based estimators 
developed here can incorporate all these measurements to produce good estimates 
and decisions. 

Overall, the constraint estimation approach to understanding contact force seems to 
give good results. The next chapter brings constraint feature models together with 
temporal features to perceive tasks. 



Tasks 



Chapter 9 



This chapter applies the ideas of the previous chapters to two tasks: tracking a 
guarded move, and following a maze. These tasks pull together all the work in the 
previous chapters. Constraint, high frequency temporal models, and velocity are 
used in distinguishing contact cases. In addition, the graphs are action dependent. 

These experiments illustrate the utility of the method and bring out some future 
issues. The main utility is that programming becomes a problem of connecting up 
measurement models instead of accurately predicting measurement forces. This is 
much easier to do by hand and is potentially much easier to do with a computer. In 
addition, the models make it possible to base decisions on multiple measurements 
which is much more robust than a single measurement threshold. Furthermore, the 
number of measurements needed to support a decision is dynamically determined by 
the measurement probabilities. 

The experiments also point out some issues that were not investigated in this thesis, 
but which are important to intelligent, autonomous, manipulation. First, actions 
which are designed to achieve goals do not necessarily gather sufficient information 
for making decisions. This problem arose in one guarded move task, when the com- 
manded force did not explore the possible alternatives. In this case, the decision 
algorithm was forced to make decisions against noise and, as expected, returned ar- 
bitrary results. Therefore when designing an action selection procedure the trade-off 
between information gathering and goal achievement must be accounted for. Fortu- 
nately, the feature graph provides a starting point for this analysis. 

Second, the system is robust to noise which fits within the context of the graph. 
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However, if there are unmodeled surfaces or things which can cause unmodeled events 
there is a tendency for the decision processes to proceed down the feature graph. The 
procedure can be made more robust by considering longer and more measurement 
paths. In addition, it may be possible to detect this error by again applying change 
detection theory to detect a mismatch between all the best current path traces and 
the measurements. 

Lastly, there is the issue of feature graph complexity. Although only a planar task, 
the configuration-space maze has 63 distinct configuration space surfaces, and 148 
feature models. Higher dimensional problems can be expected to have exponentially 
more models. There are three issues involved with this complexity. First, all the 
observer cares about is the graph branching factor. If this is small, the observer will 
still be able to track the state of the manipulation task. This should grow slowly 
with dimension. Second, most of the feature nodes in the graph will never be visited. 
For each range of starting configuration, each action will cause the robot to visit only 
a few of the feature partitions. If the graph for this set of starting configurations and 
action could be computed on-line, then the graph could be expanded as needed which 
would greatly reduce storage requirements. Lastly, even if the graph is computed 
on-line there is a need to be able to produce the graph from simulations to decrease 
the programming burden. 

The hrst set of experiments looked at guarded move strategies. Guarded moves that 
contact one or many surfaces are a common component of a multi-step manipulation 
strategy. Section 9.1 will show how the measurements experienced in the typical 
guarded move can be encoded. It will also show how the system is easily able 
to distinguish the correct contact surface even when presented with closely related 
alternatives if there is sufficient information. First, a single step guarded move 
is given as an initial illustration of the technique. Then, multiple guarded moves 
and guarded moves which contact a sequence of surfaces are presented as a direct 
extension. 

Section 9.2 applies the approach to tracking the position of the robot in a maze. The 
maze is an H shape with a single extra tail and is meant to simulate the constraints 
created by a gear-shift. The commands in this example are again forms of guarded 
moves. This example illustrates how the technique scales to actions which cause long 
sequences of features to be visited. It also illustrates how a collection of actions can 
be encoded as a collection of graphs in order to navigate the robot through desired 
task states. 

In all of the experiments the action was applied to the robot for a fixed length of time 
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and the measurements were recorded. The measurements where then analyzed by the 
host computer off-line and displayed. The constraint model estimation algorithms 
are numerically intensive and do not yet run in real-time. Also off-line computations 
were much easier to debug. With some code restructuring and optimization the 
program could be made to run in real-time on current microprocessors. 

9.1 Tracking a Guarded Move 



Three guarded move experiments were performed. The first, example simply moved 
the robot from free space into contact with a vertical surface. A small exploratory 
motion was added onto the basic motion, and this was used to show that the system 
could correctly determine the contacted constraints when presented with multiple 
possibilities. 

The second is a single command that moved the robot from free space to contact 
with a vertical surface, to contact with a wall, and then finally into a corner-all with 
a single command. This example illustrates the decision algorithm's capabilities in 
tracking a long sequence of events. The final guarded move accomplished the same 
sequence of three contacts, but performed the task with three separate commands. 
This example illustrates changing the graph connectivity based on the commanded 
action. 



9.1.1 Guarded Move Baseline: Move until Contact 



As a baseline case the system was tested on a single step guarded move. The PHAN- 
ToM was commanded to move the fingertip downward from free space onto a hor- 
izontal surface. This is the simplest possible guarded move and is just the classic 
move until contact. All the measurements were collected with the fingertip sensor 
on the PHANToM (figure 1.2). Motions were generated under automatic control. 
For this task, the low level controller was a position controller. A small sinusoidal 
exploratory motion, in all three directions, of amplitude 2 mm was added onto the 
downward command. The measurement models used in each experiment will be dis- 
cussed and a single letter abbreviation will be introduced for the model. Feature 
models then consist of triples of these letters one letter - or each base feature model: 
high frequency, constraint, and velocity. The resulting graph is provided and labeled 
using these letter triples. 
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The high frequency signal was separated into two models. The hrst model was for 
the trailing edge of impact signals (I), the second model was a general, statistical, 
stationary signal model. The moving case treated in chapter 7 was used as a base- 
line, but the expected standard deviation of the mean was increased to 2.5 based 
on observation of the measurements when performing this task. This allowed this 
single model to capture all of the stationary signal conditions in the high frequency 
measurements. This model will be referred to as the stationary (S) model. 

The Cartesian constraint models were used with the prior penalty discussed in sec- 
tion 8.3. Four constraint models were needed for this experiment. Free (0) is the 
constraint model for free space. Vertical constraint (Z) is the constraint model for 
contact with the surface. Two alternative constraints - a two constraint situation, 
one in x and one in z (XZ), and a single constraint in (Y) were also provided. 

A feature model was also created for the velocity signal. The hrst model was a vector 
normal distribution with zero mean and variance corresponding to the measurement 
noise. This model is for zero velocity (F). The second model was a uniform distribu- 
tion on all the velocities over the interval [-0.01,0.01] (M). The combination of these 
two models provided a test for deciding if the robot was moving or not moving. 

The decision parameter for rejecting a path was set to 20.0. Any path whose likeli- 
hood was 20.0 worse than the current best path was eliminated. Since a likelihood 
of 20.0 corresponds to a probability ratio of exp(— 20) this was felt to be reasonable. 
This value was small enough that only a few (less than 5) paths were maintained. A 
difference of 5.0 would be more aggressive and give faster performance. 



The feature graph for this task is 






which was defined in a hie and then read into the program using a simple parser. 
The features corresponded to 
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Feature High Freq. Model Const. Model Vel. Model 
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1 


I 
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S 
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s 






F 





M 





M 


z 


F 


xz 


F 


YZ 


F. 



The expected measurement sequence was — > 1 — > 2 — )• 1 — )• 3. The robot starts 
out fixed in free space. The initial acceleration to start the motion generated an 
impact. The trailing edge of the impact occurs while moving, therefore the impact 
is considered to be in a moving velocity state. The robot is then smoothly moving 
and contacts the surface. This causes an impact (back to feature 1) which then 
transitions to the final constraint model (3). Features 4 and 5 were provided to test 
the system in order to see if the decision algorithm would become confused. The 
system marked 10 trial runs correctly. Three runs contacted wood, three contacted 
aluminum, and four contacted hard plastic. 

Figure 9.1 shows the motion and the processing for one of the trial runs. For this 
example, most of the changes occur at impact events. An impact event is even 
detected during the contact with the surface. This is probably caused by a surface 
burr. After detecting the event, the system Initially selects feature 4 as more probable 
but quickly returns to the correct feature, number 3. Note that a simpler feature 
graph ofO— > 1 — > 2 — > 1 — > 3 would probably also have worked, and would have 
made the segmentation faster. 

To give the system a more difficult test, the alternative feature (4) was modified to 
bring it into closer alignment with the correct choice (3). As can be expected from 
the constraint experiments, the system performed correctly with only a 5 degree 
difference in the frames. When the difference was reduced to 1 degree the system 
still performed correctly, but now it took more measurements (300) to make the 
correct decision. This is the advantage of a model driven approach. Closely related 
models will require lots of data to make decisions and this will be reflected in the 
feature probabilities. 

However, it should be emphasized that the system performs well in this experiment 
because of the exploratory motion added onto the basic motion. Without exploration 
the system would not be provided with sufficient data to make a decision. Although 
Z would definitely be put in as a constraint direction, the system would not be able 
to determine if the other directions were free or constrained. If provide with these 
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Figure 9.1: Example simple guarded move trace. The top plot shows 
the position of the fingertip. The x, y } and z plots are labeled in the 
hgure. The next plot shows the force on the fingertip with the same 
line-type convention. The next plot shows the logarithm of the strain 
difference energy. The bottom plot shows the probability traces for 
each feature. 
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Figure 9.2: The fingertip on the PHANToM , represented by the sphere, 
is commanded to move down and back, with a single command, using 
position control. The constraint surfaces are labeled as (Z), (YZ), (XZ) 
and 3. The command takes the fingertip from 3 to Z to YZ to 3. 



alternatives as possible decisions, it will make arbitrary decisions against what is 
essentially noise. An experiment discussed in the next section illustrates this issue. 



9.1.2 Guarded Moves with Multiple Transitions 



The second experiment was a single guarded move that was designed to contact three 
surfaces in succession. The PHANToM is presented with a flat surface on which there 
are two walls forming a corner (See figure 9.2). There are five possible constraint 
conditions abbreviated as: f ) for no constraints, 2) Z for a single vertical constraint, 
3) XZ for a x and a z constraint, 4) YZ for a y and a z constraint, and 5) 3 for all 
possible constraints. The desired motion took the system from — > Z — > Y Z — > 3. 

Four models were used for the high frequency signal. Stationary (S) for the signal 
when there was no movement, an impact trailing edge model (I), a moving in free 
space model (MF) and a moving in contact model (MC). The parameters of these 
models were chosen based on the measurements. The constraint model for each of the 
impact signals was chosen as the constraint surface to which the impact transitions. 



Because of the low stiffness of the position controller, the fingertip underwent stick- 
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Figure 9.3: Integral endpoint force controller with position feedback. 



slip as it moved across the flat surface. This had to be explicitly accounted for in 
the feature model. Sticking was modeled by allowing transition from (MC, Z, M) to 
a (S, 3, F). Then slipping was modeled by adding a connection from (S, 3, F) back 
to (MC,Z,M) possibly through an impact mode, thus the stick-slip that occurs as 
the fingertip moves along the surface is modeled a multiple transitions between these 
two . 

Although this works reasonably well, a better approach was to change the underlying 
controller. An integral based endpoint force controller was added in parallel to the 
position controller (Figure 9.3). With this controller, the desired force input can 
be used as the basic command. In free space, this command becomes a constant 
velocity command. In contact, the controller will maintain a constant force against 
the constraints if the constraints support the desired force. A constant velocity will 
result from the forces that are not supported by the constraints. The controller acts 
like a generalized damper with a very narrow velocity error cone. All the remaining 
experiments given in this section and in the maze were performed with this controller. 



With this controller, the desired action becomes the command (100,-100,200) where 
the command is a vector desired force to achieve in mN. The feature graph for this 
task and action is 
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Given the starting condition, the expected feature sequence is — > 1 — > 8 — > 2 — > 
3— > 9 — >■ 4 — > 5 — >■ 10 — > 6. The impacts in the sequence could be missed which 
is accounted for in the feature graph. This sequence, with or without the impacts, 
was reliably found by the system. The errors that did occur happened when an 
unmodeled event occured, such as catching on the wood, which occasionally forced 
the system down the feature graph prematurely. The system can be made more 
robust by maintaining more paths. 

The same experiment was also done with the addition of a feature for the (XZ) 
constraint presented by the second wall. In this case the decision process is being 
asked to determine if the second guarded move terminated on the XZ wall or the YZ 
wall. Under integral force control the decision processor would often transition to 
the XZ constraint model while still moving along the flat Z constraint surface. 

The problem occured because the force controller made the system move directly 
in the negative y direction. No motion or force information was gathered in the x 
direction. The decision processors was therefore forced to decide between Z constraint 
and XZ constraint without any significant measurements. Any small burr or just the 
grain in the wood surface making up the Z constraint would trigger a transition and 
often the XZ constraint direction would be chosen. 

This problem could be detected by a more detailed examination of the covariance 
matrix used in constraint estimation. When this problem arises, the covariance 
matrix will have only two significant eigenvalues. The remaining four will be small 
in size. Two represent the true free directions, in the joint force and position space, 
and the four represent the lack of sufficient measurements. 
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In addition, this is not a problem intrinsic to the decision process. Rather, it is a 
higher level problem associated with action selection. The action selection processes 
must take into account not only the command required for reaching a given feature 
node but also all the features to which the system could next transition. If the 
features to which the system could transition can be disambiguated with the base 
action no exploration is necessary. However if the feature graph has multiple possible 
new features, sufficient exploration has to be added to the action. The need to explore 
can potentially be determined from the current markings on the feature graph and 
the transitions which can occur given the base action. 

However, more work is needed in control to make exploration actually work. The 
integral force controller was used for controlling the contact forces because it gives 
measurements that are easier to interpret than the position controller, because it 
does not under go stick-slip. Furthermore, it is much easier to command a force 
when the position of objects is not well known. However, exploration using this 
controller is difficult. Any exploratory position command will be rejected by the 
controller. If the position exploratory command attempts to move the robot in a 
direction in which the robot is trying to control to zero force no motion will occur. If 
the force exploratory command is added, the robot will undergo a random walk on 
the surface, due to friction, and it will be impossible to predict the expected feature 
transitions. It appears that additional work in force control is needed to develop a 
hybrid control technique that combines integral force control and impedance position 
control is needed. 



9.1.3 Action Dependant Graphs 



Transition from free space to the corner can be made more robust using a sequence of 
three guarded moves. The decision process can also be made very robust by making 
the transitions in the feature graph depend upon the desired action. The resulting 
action dependent feature graph is shown in figure 9.5. The action dependent arcs 
provide a significant amount of information. If the current command is 0, vibration 
or tapping impacts will not trigger transitions to feature (S, Z, F) because this feature 
is not connected. Furthermore, the dependent arcs bound the number of transitions 
that can occur for each guarded move. Since each feature node, within the context 
of a single guarded move, is easily disambiguated, the decision process did not make 
any errors in any of the 20 trials. 

One of the big advantages of the feature approach is that it is firing a sequence of 
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Figure 9.4: The PHANToM fingertip, represented by the sphere, follows 
a path created by three separate actions. 
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Figure 9.5: Feature graph for three step guarded move using a force 
controller. Each graph is labeled by the actions which make it active. 
The actions change the edges of the graph. 
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model estimators. The constraint estimators return estimates of the contact config- 
uration and thus the object configuration. A complete robot control system could 
exploit this information to make the guarded move strategy more robust to angular 
misalignments. 

An analysis of sticking shows that the these three actions will place the fingertip in 
the corner for angles between — 10° and 45°. The angle is measured about the z axis 
from the nominal alignment of the YZ constraint wall with x. For angles less than 
— 10° the fingertip will stick to the YZ wall. For angles greater that 45° the fingertip 
will hit the XZ wall from the YZ wall. Both outcomes could be added as possible 
features. 

For angles less than — 10° active exploration would determine that the system is in 
a single constraint situation and would return an estimate of the wall angle. The 
commanded action could then be adjusted and the system would again slide into the 
corner. In the other case, exploration would determine that the system is on the 
wall and not in the corner, and again the command could be adjusted to slide the 
fingertip into the corner. 

This sort of approach introduces higher level feedback into the system. Actions 
depend upon the feature parameter and model estimates, and the feature graph 
depends upon the actions. Doing this intelligently without introducing instabilities 
remains as a major research challenge. 



9.2 Following a Maze 



Finally, a configuration space maze was used as an example of medium complexity 
to show how the system can be used to encode up a task with many features and 
several actions. In this task an H shaped maze with an additional tail was milled out 
of Lexan. The shape and the resulting configuration space are shown in figure 9.6. 

The circular arcs in the configuration space are formed by contact of the fingertip with 
the corners of the maze. These should be modeled as circular constraint features. 
However, they were approximated as two linear constraint models because of the 
current limitations of the constraint estimator implementation. 

The free space also has to be divided into components. As discussed in section 5.3.3 
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Figure 9.6: Gear-shift maze and its configuration space. The maze 
was milled out of Lexan to a depth less than the radius of the fingertip 
endtip sphere. The circle formed by slicing the sphere at the given depth 
is the moving object in this planar task. Together the two objects form 
the indicated configuration space. 
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Figure 9.7: Simplified configuration space partitions. There are 62 basic 
configuration space partitions in this task after simplifying the back- 
projections and the curved surface. In addition, the eight basic force 
commands are shown in the lower right. The connectivity between the 
configuration space partitions are shown for action f . 



this should be done by considering the back-projections of each feature under the 
desired actions. Switching from one action to another entails hrst instantiating a 
second feature graph consisting of both new edges and nodes, and then setting the 
initial probabilities of all the new nodes. These probabilities are computed by cover- 
ing the partitions associated with the nodes with non-zero probability in the original 
graph with partitions from the new graph. For this task, this was a bit too complex. 
Instead, the back-projections were computed and then heuristically unified into a 
fixed set of partitions. Different actions can then be encoded by just changing the 
connectivity of the fixed set of nodes. Figure 9.7 shows the 62 configuration space 
partitions that result, and the eight basic commands. 
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Each action induces transitions between the configuration space partitions. Fig- 
ure 9.7 also shows the transition for action f. The other actions produce different 
transition graphs. As expected, the figure shows that action f separates the features 
into three different regions. Action f will take the robot from partition 57 through a 
series of partitions to partition 5. It is also possible that the robot will become stuck 
at partition 23 or partition 20. 

A simulation of the robot as a generalized damper with friction could be used to 
compute the transition graph. Multiple Monte Carlo simulation could be run to 
generate the possible transitions and even assign transition probabilities. 

The feature graph is derived from the transition graph for the partitions. Figure 9.8 
shows the feature graph for the statistically stationary feature models and the impact 
models. Each statistically stationary feature is labeled with a constraint model and 
either (f ) for fixed or (m) for moving. Impact models have the constraint and velocity 
model of the stationary model they terminate in, and the models have not been put 
on the label. 

To determine the stationary models, each of the configuration space partitions was 
simply doubled. One feature is for being in the configuration partition and not 
moving, and one feature is for being in the partition and moving. Impacts were 
introduced when the motion from one partition to a second partition would involve 
an abrupt change in constraint. Thus there is an impact model from f 08 to f 09, and 
from 90 to 80. 

Our experiments focused on commanding action f from node 57. The most complex 
sequence of events occurs for this action-node combination. This sequence of events 
also appears in moving from 60 to 8, and from 5 and 8 under action 5. In this event 
sequence it is important to pick out feature 90 and 91 because these signal that the 
system has entered the "neutral" position of the H. If the system desired to move 
from feature 57 to feature 62 it would change action at one of these features. 

Most of the connections between the stationary models come from the connectivity 
of the partitions. However, to make the system robust it was necessary to add 
additional connections. Certain features are very clear in the force and position 
signal traces because they are either distinctly different from the previous features or 
they come from persistent contacts. For action f , starting from 57, feature 108, 109, 
90, 81, 71, 116, and 5 are all very distinct. To make the system robust, we added 
extra connections from distinct features to the next distinct feature. For example, 
the transition from f 08 to 90, under the basic connectivity from the action, requires 
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Figure 9.8: Feature graph for action 1. 
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passing through 104 which is not distinct. Errors can occur if 104 must be detected 
before 90 can be hypothesized. By adding a connection from 108 and 109 to 90 the 
system is made more robust. Similarly, 77, 82 and 83 are nominally required nodes 
from 81 to 71. These can also be missed. By adding a connection from 81 to 71 the 
system is made more robust. 

In addition, the nominal model is that the initial motion from 57 transitions directly 
to 108. However, unloading the force from partition 5 in the corner with command 
1 appears as a transition from 57 to 52. Adding these unloading transitions also 
improves performance. 

This information can really only be gleaned either from a really good simulation 
or from actual measurements. In the HMM approach to both speech and force 
processing [Hannaford and Lee, 1991] the transitions are learned from repeated trials 
using the EM algorithm. A similar approach should be applicable here and would 
significantly enhance robustness for repeated tasks. 

With these additional connections, 20 experimental trials from 57 under action 1 
were correctly marked. The return from 5 to 57 under action 5 was also correctly 
marked. A typical feature trace is shown below. Each column corresponds to the 
order list of feature models considered possible. 

57 52 108 108 109 109 109 90 81 81 71 116 5 
52 109 128 104 124 82 

128 



9.3 Conclusion 



This chapter discussed experiments with a system that observes all of the features 
discussed in the previous chapters. The observer uses constraint models, high fre- 
quency temporal models, and a simple model of velocity. Triples of these models 
are the basic feature model. Graphs of these feature models are formed from the 
connectivity of the underlying configuration space under a given action. This graph 
encodes the sequence of measurement models that can be expected given an action, 
which can then be used to observe where the robot is in configuration space for a 
given task. Different actions give rise to different graphs. 

The possible measurement models are determined by the range of constraints that 
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can arise in a given task. The connectivity depends upon both the connectivity 
imposed by the action, and on what will actually be observered during the task. 
Features which are visited for only a short time or which are ambiguous can be 
missed, and extra connectivity needs to be added to deal with this possibility. This 
extra connectivity is best determined by observing actual examples. 

We showed that the feature observer was able to use the information encoded in 
the feature models and feature graph to track the progress of several tasks. This 
capability can form the basis for manipulation programming. 

Selection of actions given the feature probabilities assigned by the feature observer is 
the other major component of a complete, intelligent, manipulation program. This 
action selection procedure must take into account both the basic action needed to 
accomplish the desired goal and the possibility that exploration may be required to 
determine the current feature state. Given a basic action, the feature graph shows 
where exploration is required, and the constraint covariance matrix indicates the 
directions in which there is insufficient information. 

Several other important extensions to this work are indicated by the difficulty of 
creating the feature graph. Monte-Carlo simulation of the robot and environment 
would seem to be an important hrst step in automatically computing the feature 
graph. Learning using repeated trials and EM would also significantly enhance ro- 
bustness and eliminate the need for hand programming both the initial feature model 
parameters and the connections. 

The demonstrations discussed in this chapter show that contact features can be 
automatically detected and used in manipulation based programming. This is a step 
toward building a completely autonomous and automatically programmed system 
that incorporates contact information. 



Conclusions 



Chapter 10 



This thesis presented a feature-based approach to observing the state of a manip- 
ulation task. Contact features were defined as predictive, statistical, measurement 
models which produced a countable partition of the task phase-space. This definition 
was contrasted with the idea of confuseable sets. 

The statistical approach has two implications. First, the statistical definition im- 
plies that the robot can only observe its current state probabilistically. There are no 
guaranteed states, only high probability states. Second, the definition makes it pos- 
sible to use the machinery of stochastic parameter estimation and change detection 
theory. Since this is a well formed body of knowledge, powerful tools are available 
for solving a contact task observer problem formulated in this way. 

These tools were used to develop a statistical decision model which tracked measure- 
ment paths given prior knowledge encoded in the expected model parameter values 
and feature graph. The approach can be seen as a system which runs a collection of 
matched Kalman filters on the measurements. The Kalman filters take the form of 
model parameter estimators. The innovations produced by the filters are monitored 
by generic cumulative sum change detectors. These change detectors quickly and ro- 
bustly locate deviations in the innovations process from whiteness. When a change 
is detected, the system determines which new Kalman filters to run by examining 
the feature graph. A filter is started for every node in the graph to which the system 
could have transitioned given the starting node. The complete observer estimates 
the most likely paths for the measurements through the feature graph. This was 
related to the probability of being in any feature in the feature graph, which solved 
the basic feature observer problem. 
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Feature estimators were then developed for two basic features: temporal sequences 
and constraint relationships. The constraint estimator combined the velocity and 
force information in a uniform manner and treated the estimation problem in the 
joint velocity and force space. This unified the development of the estimator and 
made it possible to develop a single detection algorithm for the number of degrees- 
of- freedom and all directions constraint. In addition, by including the curvature of 
the configuration space we were able to formulate an estimator for planar polygons 
which could recursively incorporate all of the measurement information. This esti- 
mator determines the geometric parameters describing the contact without requiring 
knowledge of the relative configuration between the robot and the environment. The 
relative configuration can be computed from the current robot configuration and the 
estimated geometric parameters up to equivalence in the parameters. 

Finally, a demonstration of the complete theory was implemented for a sphere in 
Cartesian space. This system used the force measurements from a 6 axis force- 
torque sensor mounted on a smooth, force controllable robot (the PHANToM). The 
PHANToM is able to position the sensor in Cartesian space. Several experiments 
were performed with this system. These experiments showed that the system was 
able to track the sequences of features encoded by the feature graph. A number of 
lessons were learned from the implementation, and these motivate the discussion of 
future work. 



10.1 Future Work 



The work in this thesis laid the groundwork for a contact feature approach to ma- 
nipulation programming. A number of significant steps were suggested by the im- 
plementation and experiments. 



10.1.1 Computational Construction of Feature Graphs 



The feature graphs become quite large quite quickly. To make the construction of 
these graphs practical, a program should be developed which can quickly compute the 
expected graph for a nominal plan and action. This leads to a two level system. The 
high-level system computes nominal trajectories and the expected feature contacts 
and transitions that could occur. The feature graph is then down-loaded to a faster 
process. Some of the features are labeled as terminal features where the system would 
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like to consider a new action. This process monitors the measurements and looks for 
transitions from the current feature and determines when a terminal feature has been 
reached. Creating this high-level system is still a difficult computational problem, 
but because features encode large regions of phase-space the problem should be easier 
then reasoning based on point sets. 



10.1.2 Task Control and Exploration 



As has long been recognized, force control is needed to control the robot when there is 
very large uncertainty in the task geometry. Integral force control seems to provide 
the best explicit force control. However, with friction, this appears incompatible 
with exploration of a contacted surface. It may be necessary to use switching hybrid 
control laws. The difficulty is that the switching is determined by the feature observer 
and the information gathered by the observer depends upon the control law. This 
means the problem is a switching control feedback problem and the issue of stability 
becomes paramount. 

It is important to determine when exploration is needed and what form it should 
take. The feature observer and graph provide two pieces of information that could be 
used in the analysis. First, the information matrix used in the constraint estimation 
procedure indicates when there is insufficient information in the measurements to 
make a robust decision. Second, the distribution over the feature graph and the 
possible transitions from the current set of possible nodes indicates hrst when the 
system does not know where it currently is, and second when the next transition 
could be confuseable given the current action. 



10.1.3 Feature Graph Learning 



Lastly, although computational construction of the feature graphs is important to 
solving new problems it may not be sufficient. Accurate simulation of the outcome 
of an action is difficult and for repetitive tasks the task itself is a better simulation. 
In addition, it is difficult to predict which transitions the observer may miss and 
therefore where extra graph connections need to be added. 

Given an initial guess at the appropriate features, maximum likelihood in the form 
of the Expectation Modification (EM) algorithm could be used to refine the graph 
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over multiple trials. In the implemented experiments, the experimenter essentially 
acted as this algorithm adding edges to make the system more robust. 



10.2 Conclusion 



The feature approach to manipulation sensing developed in this thesis is a step toward 
a method of robot programming that uses models of contact instead of just looking 
for force thresholds. This is a significant departure from the original paradigm for 
guarded moves. Models are much easier to write down for the task than the forces, 
because they are intrinsic to the task and are not a function of the forces applied 
by the robot. Furthermore, models make it possible to filter and make decisions 
over sequences of measurements. This approach is much more robust than making 
decisions over single measurements, and additional information about the contact 
conditions is automatically extracted by the estimation procedures. The approach 
requires further development to make it faster and to add actions. Development of 
a tight connection between the observer and actions will provided a new way to do 
manipulation programming. 



Mathematical Background 



Appendix A 



A.l Homogeneous Coordinates and Constraints 



This section reviews homogeneous coordinates and transforms. 



Let p G 3£ n be the coordinates of some point. A transformation is a map T : ffi 1 — > ?R. n 
which preserves distance. Any such transformation can be written as a rotation plus 
a translation. The transformation T takes p to p as 



T(p) = P = Rp + r. 



(a.i; 



Transformations are not linear maps on ?R. n because T(pi + P2) 7^ T(pi) + T(p 2 ). 
Transformations can be made linear by embedding ?R. n in ?R. n+1 via p — s 
In homogeneous coordinates, a transformation is given by 



R r 

1 



(A.2) 



and P = Tp. The inverse of a transform T is 

T -i 

since R is an orthonormal matrix. 



R T 





R T r 

1 



(A.3) 
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A rigid body B consists of a collection of points. Their locations are defined relative 
to a local coordinate system. Their coordinates relative to any other coordinate 
system is obtained by applying the same transform T to every point in the body. 
Therefore, the configuration space of a rigid body is the space of transforms. In 
mathematical notation C = ?R. n X SO(n). The tangent bundle and cotangent bundle 
are defined on this configuration space. 

In ?R. n the dual object to a point is a hyper-plane of dimension n — 1. This notion 
carries over to homogeneous coordinates. The homogeneous coordinates of a hyper- 
n 



plane are n 



-d 



A. 2 Estimation and Filtering 



The hrst step in building an observer is to develop algorithms for estimating the 
feature parameters given the measurements and to determine the likelihood of the 
measurement sequence. When the parameters are free (no prior distribution) the 
maximum likelihood estimator (MLE) provides the necessary computations. In 
matching, a prior distribution is provided, and this information must be folded in 
with the information from the measurements. The Kalman filter provides the nec- 
essary computations. In many cases, the feature parameters will not have a normal 
prior distribution on the natural regression directions because they are correlated 
in a nonlinear manner. However it may be possible to orthogonalize the regressors 
and produce orthogonal parameters which do have approximately normal distribu- 
tions. We therefore, complete this section by showing how the regressors can be 
orthogonalized within the Kalman filter. 



A. 2.1 MLE Estimation of Feature Parameters 



We will consider only linear, Gaussian, predictive measurement models with additive 
noise. The general form of this type of model is 

y(t) = t/,(t)0 + v(t) (A.4) 
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where ip (t) is a matrix of regression coefficients, is the parameter vector, and v 
is a white Gaussian noise process, y is a vector of size m. 1 

When we have no prior information about the distribution of 0, the maximum like- 
lihood estimate (MLE) provides the best basis for estimating and detecting changes 
in the parameters. For the linear predictor model, the MLE estimate of the param- 
eters is the least-squares estimate. The estimate can be written in matrix form by 
collecting the measurements into a vector as Y(n) = y™ , and collecting each column 
of t\} into a vector as s;(n) = V'i(0t l =i- Then the least-squares estimate takes the 
form 

(A.5) 
(A.6) 
(A.7) 
(A.8) 

(A.9) 

where (|) denotes inner product. I is called the empirical information matrix and X 
is called the empirical information vector. 0(n) is the parameter estimate at time n. 

The parameter estimates depend upon the model order because in general the vectors 
{s;(n)} are not orthogonal. An orthogonal set of parameters, sometimes termed the 
reflection coefficients [Makhoul, 1975], can be formed by first orthogonalizing the 
vectors {s;(n)}. The orthogonal parameters can then be used to estimate 6 for any 
desired order. 

When the computation is performed on-line only I(n) and X(n) are actually kept 
in memory. Therefore, the reflection coefficients need to be computed from these 
matrices. If the vectors {s;(n)} were actually available, a Gram-Schmidt procedure 
could be applied to the vectors to generate an orthogonal basis. This decomposition 
represents the matrix S = [s;] as S = QR T where Q is the orthonormal basis, and 
R is the correlation structure of S. The time dependence has been dropped for 
clarity. Then, the unnormalized reflection coefficients, k, satisfy k = Q T Y. 

Now note that I = S T S = RR T , since Q is orthonormal, and X = S T Y = RQ T Y. 

Therefore the unnormalized reflection coefficients are generated by the hrst stage 
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^^Much of this material can be found in advanced texts on signal processing and estimation 
[Anderson and Moore, 1979]. 
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of Gaussian elimination. Gaussian elimination factors I into RR . Therefore, the 
reflection coefficients are the solution to Rk = X. This solution is just the result of 
the first stage of Gaussian elimination. The second step, back substitution, solves 
R T = k. 

Now the unnormalized reflection coefficients can be used to reconstruct the solution 
for any model order. Given any order model ra, let the hrst ra coefficients be k m and 
the corresponding sub-matrix of R T be R^. Then the original model coefficients for 
an order ra model can be determined from k m = R^0 m . 

A square-root algorithm provides an efficient, numerically stable procedure for com- 
puting the coefficients. This procedure works directly with R. When a new mea- 
surement y(n) is taken the following update is performed: 



f . Update the regression matrix tj){n) based on the model equation. 

2. Update R by finding a Householder transform T such that 

[R(n) 0] = [R(re-l) ip T (n)]T. (A.fO) 

Since T is orthonormal, this equation is factored 

R(n)R T (n) = R(n - f )R T (n - f ) + rp T rp (A.f f ) 

which is the update for the information matrix. 

3. Solve 

R(n)[k(n) V>(n)] = [X(n) ip(n)]. (A.12) 

4. If desired, solve for the model parameters of length ra. 

m <- R" T k m (A.13) 

5. Compute the residuals in the orthogonal frame. 

v i( n ) = ^j-i(n)-'ik j (n)k j (n) (A. 14) 

with vo(n) = y(n). (A. 15) 
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6. Finally, compute an estimate of the log-likelihood 2 of the residual using the 
estimated variance 

l(y(n)\yT V ) = Kv{n)) = -i(logdet V„(n) + v(n) T V v (n)-\(n)f (A.16) 

The measurement sequence log-likelihood is estimated by summing these terms. 

7. Compute an estimate of the variance of the residuals via: 



1 



\ v {n)} = V v (n - 1) + -(u(n)u(nY - \ v {n - 1)) 



(A.17) 



which could also be done with a square-root update algorithm. 



A. 2. 2 Estimating Parameters with Prior Values 



When a prior distribution is provided for the feature parameters, this information 
must be incorporated into the estimation procedure. The Kalman filter incorporates 
the information and produces a white innovations process that can be used for testing 
for changes from the model. The square-root implementation of the Kalman filter 
provides a robust, efficient, implementation of the filter. Our review of the algorithm 
follows [Willsky, 1990] . The development of the algorithm shares many similarities 
with the previous estimation algorithm. 



The state and measurement equations can be written in matrix form as 



y{n) 

[0(n + l)\ 



Id 



V 



1/2 



V 





0(n) 



(A.18) 



where fi(n) is a white, normal, process with identity covariance. Given, Q(n\n — 1) 
n yit,\it, — f) the measurement prediction and parameter update step are 



and V/i (n\n 



y(n\n — f) 
6(n + l\n- I) 



Id 



e(n\n-l) 



(A.19) 



2 The log-likelihood is the log of the conditional probability of receiving the measurements given 
the model. 

3 In this case it is actually possible to compute the measurement sequence log-likelihood exactly. 
If Eq = (Y(n)|Y(n)} is the total energy in the signal, the exact value of the log-likelihood is 
-n/2(log((£o - k(n) T k(n))/n) + m). 
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These two equations can be combined to compute the innovations and the error in 
parameter estimate 



where 



rj has identity covariance. 



v(n)' 




*(n 


)Vi/ 2 (n|n-l) \]i 2 ' 


\ 




V^ /2 (n|n-l) 




T](n) = 


'\~ e ll2 e{n\n-l) 





rj(n) 



(A.20) 



(A.2i; 



Now we apply a Householder transform T such that 



*(n)VJ/ 2 (n|n- 
V/i (n\n — 1] 



1) V 



1/2 
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Then 
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IF 
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[b d\ 
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OL 

./3. 


= 1 
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tW 



(A. 22) 

(A.23) 
(A. 24) 



F = \]i 2 (n\n) 



Using these new dehnitions yields 

E[i/(n)i/(n) T ] = FF J 
E[0(n + l\n-l)j/(n)] = BF T . 

Therefore the new parameter estimate is given by 

0(n + l\n) = 0(n + l\n-l) + E[0(n + l\n-l)j/( 
E[i/(n)i/(n) T ]" 1 i/(n) 
= 0(n + l\n-l) + Ba. 

Finally, the covariance of the parameters is also produced since 

0(n + l\n) = 0-0(n + l\n) 
= D(3 

-> \^ 2 (n + l\n) = D. 



(A.25) 
(A.26) 



(A.27) 
(A.28) 



(A.29) 
(A.30) 
(A.31) 



The algorithm is summarized below. 
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1. Given 01 



ra ra 



1), V 



1/2, 



ra ra 



1), Vjy (ra|ra — 1), VK n ) form 



1/2/ 



rp(n)\n (ra|ra — 
\J (ra|ra — 1] 



1) V 



1/2 



1/ 





(A.32) 



The initial values of the estimates and the covariance matrices are provided by 
estimating the covariance and parameter values over a set of training examples. 



2. Apply a Householder reduction T to reduce A 



AT 



-^1/2/ I > 

Vj> (n\n) 
Kin) 



Vi/ 2 (ra+l|ra) 



(A.33) 



3. Compute the innovation 



v(n) = y(ra) — }&(n)0{n\n — 1] 



(A.34) 



4. Solve for the normalized innovations a 



rl/2/ 



V|> (n\n)a.(n) = u(n) 



(A.35) 



5. Update the parameter estimates 

Bin + lira) = B(n\n - 1) + K(n)a(n) 



(A.36) 



6. Compute a statistic for change detection 



l c (n) 



--oc(n) a(n) 



(A.37) 



7. Update the estimate of the square-root of the variance using another House- 
holder reduction 

[Vl/ 2 (ra + l|ra) 0] = [(l-llnyl 2 Y)l\n\n-l) y/(l/n)u(n) } T 2 . 

(A.38) 

8. Compute an estimate of the log likelihood of the innovations 



/(, 



1 



rl/2/ 



-logdet(V^ /z (ra+l|ra)) + / c (ra) 



(A.39) 
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A. 2. 3 Estimating Orthogonal Parameters with Priors 



In order for the Kalman filter to work properly, the initial value of the parameter 
estimates and the covariance has to be at least approximately normal. As was 
discussed in the thesis, the training estimates used for the temporal models were very 
far from normally distributed. However, if an orthogonal set of parameters, generated 
by orthogonalizing the regression vectors, was used the orthogonal parameters were 
approximately normally distributed. 

In training, the values of the feature parameters were estimated using the maximum 
likelihood square-root algorithm to produce orthogonal parameters. During labeling, 
the Kalman filter needs a small modification. 

Orthogonal parameter estimates will be computed by the square-root Kalman filter, 
if orthogonal regressors are provided to the algorithm. If R(n) represents the current 
correlation structure, then the orthogonal regressors are produced by solving 

V>(n)R(n) = i/>(n). (A.40) 

The correlation structure is maintained using yet another Householder reduction T 

[R(n) T 0] = [R(re-l) rj>(n) T ]T (A.41) 

D" 1/2 (n)R(n) = R(n) (A.42) 

where D -1 ' 2 is the matrix formed from the diagonal elements of R. 

Using either of these two algorithms, signals are mapped into estimates of the feature 
parameters and an estimate of the log-likelihood that the given feature is the correct 
model for the measurements. 
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